Merge branch 'dev' into 'master'

Dev to Master Sync

See merge request ros/ros_bt_py!153

.gitlab-ci.yml

@@ -1,14 +1,28 @@
 variables:
-  CI_SCRIPTS_BRANCH: master
-
-catkin_pipelines:
-  trigger:
-    include:
-      - project: 'continuous_integration/ci_scripts'
-        ref: $CI_SCRIPTS_BRANCH
-        file: '/gitlab-ci-yml/catkin_pipeline_v2.yml'
-    strategy: depend
+  CI_SCRIPTS_BRANCH: dev
+  DOCS_SOURCE: "ros_bt_py/doc/source"
+
+.build_matrix:
   parallel:
     matrix:
       - ROS_DISTRO: noetic
         CI_IMAGE: ids-git.fzi.de:5555/continuous_integration/ci_docker_images/ubuntu_20.04_ros
+
+stages:
+  - check
+  - build
+  - upload
+  - deploy
+
+include:
+  - project: 'continuous_integration/ci_scripts'
+    ref: dev
+    file: '/gitlab-ci-yml/catkin_pipeline_v3.yml'
+  - project: 'continuous_integration/ci_scripts'
+    ref: dev
+    file: '/gitlab-ci-yml/doc_sphinx.yml'
+
+clang-tidy-check:
+  allow_failure: true
+
+

.gitlab/issue_templates/bug_report.md

@@ -0,0 +1,11 @@
+# Bug report
+
+<!-- Please fill out the fields below the best you can. A minimal example containing no project related code would be ideal so we can debug without building the whole project ourselves -->
+### Bug Description
+<!-- Insert Description here -->
+
+### Minimal Example
+<!-- Provide minimal example to reproduce the bug. Ideally you attach a tree file or a screenshot -->
+
+### Urgency
+<!-- Is this bug a big problem in your project? If that is the case let us know -->

.gitlab/issue_templates/feature_request.md

@@ -0,0 +1,11 @@
+# Feature request
+
+<!-- Please fill out the fields below the best you can -->
+### Feature Description
+<!-- Insert Description of your desired feature here -->
+
+### Minimal Example
+<!-- Provide minimal example of the problem your feature might solve-->
+
+### Urgency
+<!-- Is this feature a big need in your project? If that is the case let us know -->

.gitlab/issue_templates/node_request.md

@@ -0,0 +1,12 @@
+# Node request
+
+<!-- Please fill out the fields below the best you can -->
+### Node Description
+<!-- Insert Description of your desired Node here -->
+<!-- This includes node inputs, outputs and options -->
+
+### Minimal Example
+<!-- Provide minimal example of the problem your node might solve-->
+
+### Pre-existing code
+<!-- Is this node something you implemented in your project? If that is the case let us know and add the code below -->

.gitlab/merge_request_templates/dev_to_master_sync.md

@@ -0,0 +1,7 @@
+# Dev to Master Sync YYYY-MM-DD
+
+Before doing the sync merge make sure all items below are checked:
+- [ ] Check if commit history since last merge is acceptable and dev is rebased to include last
+      merge commit
+- [ ] Check if CHANGELOG.md is updated with all changes to be merged to master
+- [ ] Check if all changes that need further documentation are well documented

CHANGELOG.md

@@ -1,8 +1,70 @@
-# Dev Sync 20.01.23
-* Introduced Dual Tree Arguments - Launching in dual mode now allows for loading two separate trees
-* Updated ci - Removed melodic build from industrial ci (was e.o.l)
-* Added pre-commit config - Does formatting and linting, also protects master branch from commits
-* Added new NoInputParam Node - Node can get ROSParams from the parameter server without wired
-  inputs
-* Fixed code styling - Fixed a lot of formatting issues noticed by introduced pre-commit and ci
-* Added CHANGELOG.md - File to keep track of changes in the project
+# Changelog
+
+History of changes and bugfixes for ros_bt_py
+
+## [Unsynced]
+
+## [Dev Sync 08-02-2023]
+
+### Added
+
+- ROS Diagnostics - Information about the current status of the BT (ticking, not ticking, error)
+- Templates - New templates for common issue types and sync merges
+
+### Changed
+
+- Async Service Proxy - Now utilizes a singleton architecture utilizing threads instead of processes
+
+### Fixed
+
+- Action Termination - Actions now properly wait for a result before the Node returns success
+- Code Coverage - Reenabled code coverage to be extracted from ci
+
+
+## [Dev Sync 20-01-2023]
+
+### Added
+
+- Pre-commit config - Formatting and linting, also protects master branch from direct commits
+- NoInputParam Node - Node can get ROSParams from the parameter server without wired inputs
+- CHANGELOG.md - File to keep track of changes in the project
+- Dual Tree Arguments - Launching in dual mode now allows for loading two separate trees
+
+### Removed
+
+- Melodic CI support - was e.o.l
+
+### Fixed
+
+- Fixed code styling - Fixed a lot of formatting issues noticed by introduced pre-commit and ci
+
+
+[Unsynced]: https://ids-git.fzi.de/ros/ros_bt_py/compare/master...dev
+[Dev Sync 08-02-2023]: https://ids-git.fzi.de/ros/ros_bt_py/compare/ba212432...6d3e71ba
+[Dev Sync 20-01-2023]: https://ids-git.fzi.de/ros/ros_bt_py/commits/ba212432
+
+<!---
+## [Dev Sync DD-MM-YYYY]
+
+### Added
+
+- Put all Additions to the repository in here
+
+### Changed
+
+- Put all Changes in existing functionality here
+
+### Deprecated
+
+- Put all soon-to-be removed features here
+
+### Removed
+
+- Put all removed features here
+
+### Fixed
+
+- Put bugfixes here
+
+[Dev Sync DD-MM-YYYY]: https://ids-git.fzi.de/ros/ros_bt_py/compare/OLDHASH...NEWHASH
+-->

ros_bt_py/CMakeLists.txt

@@ -38,6 +38,12 @@ catkin_package(
 #  DEPENDS system_lib
 )
 
+if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING)
+  find_package(code_coverage REQUIRED)
+  # Add compiler flags for coverage instrumentation before defining any targets
+  APPEND_COVERAGE_COMPILER_FLAGS()
+endif()
+
 ###########
 ## Build ##
 ###########
@@ -98,14 +104,22 @@ if (CATKIN_ENABLE_TESTING)
   catkin_add_nosetests(test/unittest)
 
   find_package(rostest REQUIRED)
-  add_rostest(test/rostest/async_service.test)
-  add_rostest(test/rostest/parallel_if_remote.test)
-  add_rostest(test/rostest/package_manager.test)
-  add_rostest(test/rostest/ros_header.test)
-  add_rostest(test/rostest/ros_leaf_utility.test)
-  add_rostest(test/rostest/ros_leaves.test)
-  add_rostest(test/rostest/ros_param.test)
-  add_rostest(test/rostest/shovable.test)
-  add_rostest(test/rostest/shove_tree.test)
-  add_rostest(test/rostest/websocket_interface.test)
+  add_rostest(test/rostest/async_service.test ARGS coverage:=ENABLE_COVERAGE_TESTING)
+  add_rostest(test/rostest/parallel_if_remote.test ARGS coverage:=ENABLE_COVERAGE_TESTING)
+  add_rostest(test/rostest/package_manager.test ARGS coverage:=ENABLE_COVERAGE_TESTING)
+  add_rostest(test/rostest/ros_header.test ARGS coverage:=ENABLE_COVERAGE_TESTING)
+  add_rostest(test/rostest/ros_leaf_utility.test ARGS coverage:=ENABLE_COVERAGE_TESTING)
+  add_rostest(test/rostest/ros_leaves.test ARGS coverage:=ENABLE_COVERAGE_TESTING)
+  add_rostest(test/rostest/ros_param.test ARGS coverage:=ENABLE_COVERAGE_TESTING)
+  add_rostest(test/rostest/shovable.test ARGS coverage:=ENABLE_COVERAGE_TESTING)
+  add_rostest(test/rostest/shove_tree.test ARGS coverage:=ENABLE_COVERAGE_TESTING)
+  add_rostest(test/rostest/websocket_interface.test ARGS coverage:=ENABLE_COVERAGE_TESTING)
+
+  if(ENABLE_COVERAGE_TESTING)
+    set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test*,*/${PROJECT_NAME}/html*")
+    add_code_coverage(
+      NAME ${PROJECT_NAME}_coverage_report
+      DEPENDENCIES tests
+    )
+  endif()
 endif()

ros_bt_py/doc/source/development_notes.rst

@@ -0,0 +1,35 @@
+Development Notes
+=================
+
+This section describes development notes and remarks from the developers about design decisions.
+
+AsyncServiceProxy
+-----------------
+
+To effectively use ServiceProxies with BehaviorTrees a method to call ROS ServiceProxies
+asynchronously is required.
+During the initial implementation of the library a solution using Process forking was implemented.
+This is implemented in the ``AsyncServiceProxy`` class.
+Each time a service is called, the ``AsyncServiceProxy`` aiming to call a service, spawns a new
+Process that handles the Service call.
+To ensure data consistency between the Process running the ``AsyncServiceProxy`` and the spawned
+Process a Python ``SyncManager`` was used.
+
+A downside of this implementation was that for each Service call a number of processes and IPC
+channels are created.
+When working with large trees this could lead to ``Too many open files`` errors preventing the
+tree from running.
+
+Thus, a version using ``Threads`` was implemented in place of ``Processes``.
+While a ``Thread``-based solution is limited in parts by the `GIL <https://wiki.python.org/moin/GlobalInterpreterLock>`_,
+the limitations are far less severe compared to the OS resource issues the ``Processes`` solution
+expressed.
+Data is now synchronized using shared memory and a mutex.
+Additionally a singleton pattern is implemented to allow the sharing of ``ServiceProxy``-Instances
+between ``AsyncServiceProxy`` for the same service URL.
+All created ``ServiceProxy`` instances are stored in a global dict, organized by their associated
+service url and type, and can be claimed by a ``AsyncServiceProxy`` for a call.
+Importantly, they are unclaimed after each call to make them available to other ``AsyncServiceProxy`` instances.
+Should two proxies aim to call the same service simultaneously, a new ``ServiceProxy`` is created on-demand, thus
+reducing the overall resource footprint further.
+As the newly created proxy is not destroyed, on further occurrences no new ``ServiceProxy`` must be allocated.

ros_bt_py/doc/source/index.rst

@@ -15,6 +15,7 @@ Contents:
    testing_node_classes
    utility_functions
    shoving
+   development_notes
    ros_bt_py
 
 Indices and tables

ros_bt_py/launch/ros_bt_py.launch

@@ -57,6 +57,7 @@
   <arg name="load_default_tree_permissive" default="false" />
   <arg name="default_tree_path" default="package://ros_bt_py/etc/trees/examples/file_example.yaml" />
   <arg name="default_tree_tick_frequency_hz" default="1" />
+  <arg name="default_tree_diagnostics_frequency_hz" default="1" />
   <!-- available commands are:
     DO_NOTHING = 0
     TICK_ONCE = 1
@@ -70,6 +71,7 @@
   <arg name="load_default_dual_tree_permissive" default="false" />
   <arg name="default_dual_tree_path" default="package://ros_bt_py/etc/trees/examples/file_example.yaml" />
   <arg name="default_dual_tree_tick_frequency_hz" default="1" />
+  <arg name="default_dual_tree_diagnostics_frequency_hz" default="1" />
   <!-- available commands are:
     DO_NOTHING = 0
     TICK_ONCE = 1
@@ -117,6 +119,7 @@
     <param name="default_tree_path" value="$(arg default_tree_path)" />
     <param name="default_tree_tick_frequency_hz" value="$(arg default_tree_tick_frequency_hz)" />
     <param name="default_tree_control_command" value="$(arg default_tree_control_command)" />
+    <param name="default_tree_diagnostics_frequency_hz" value="$(arg default_tree_diagnostics_frequency_hz)" />
   </node>
 
 
@@ -136,6 +139,7 @@
     <param name="default_tree_path" value="$(arg default_dual_tree_path)" />
     <param name="default_tree_tick_frequency_hz" value="$(arg default_dual_tree_tick_frequency_hz)" />
     <param name="default_tree_control_command" value="$(arg default_dual_tree_control_command)" />
+    <param name="default_tree_diagnostics_frequency_hz" value="$(arg default_dual_tree_diagnostics_frequency_hz)" />
   </node>
 
   <group if="$(arg web_interface)">

ros_bt_py/package.xml

@@ -42,6 +42,7 @@
   <exec_depend>rqt_console</exec_depend>
   <exec_depend>catkin</exec_depend>
 
+  <test_depend>code_coverage</test_depend>
   <test_depend>rostest</test_depend>
   <test_depend>std_msgs</test_depend>
   <test_depend>std_srvs</test_depend>

ros_bt_py/scripts/tree_node.py

@@ -30,10 +30,8 @@
 #  -------- END LICENSE BLOCK --------
 
 import rospy
-
-from ros_bt_py_msgs.msg import Messages, Packages
-
-from ros_bt_py_msgs.msg import Tree, DebugInfo, DebugSettings, NodeDiagnostics
+from diagnostic_msgs.msg import DiagnosticArray
+from ros_bt_py_msgs.msg import Tree, DebugInfo, DebugSettings, NodeDiagnostics, Messages, Packages
 from ros_bt_py_msgs.srv import (
     AddNode,
     AddNodeAtIndex,
@@ -107,6 +105,9 @@ def __init__(self):
         default_tree_tick_frequency_hz = rospy.get_param(
             "~default_tree_tick_frequency_hz", default=1
         )
+        default_tree_diagnostics_frequency_hz = rospy.get_param(
+            "~default_tree_diagnostics_frequency_hz", default=1
+        )
         default_tree_control_command = rospy.get_param(
             "~default_tree_control_command", default=2
         )
@@ -122,6 +123,11 @@ def __init__(self):
             "~debug/node_diagnostics", NodeDiagnostics, latch=True, queue_size=10
         )
 
+        node_in_namespace = rospy.get_namespace().strip('/')
+        namespace = rospy.get_namespace() if node_in_namespace else ''
+        self.ros_diagnostics_pub = rospy.Publisher(
+            f"/diagnostics/{namespace}", DiagnosticArray, queue_size=1)
+
         self.debug_manager = DebugManager()
         self.tree_manager = TreeManager(
             module_list=node_module_names,
@@ -130,6 +136,8 @@ def __init__(self):
             publish_debug_info_callback=self.debug_info_pub.publish,
             publish_debug_settings_callback=self.debug_settings_pub.publish,
             publish_node_diagnostics_callback=self.node_diagnostics_pub.publish,
+            publish_diagnostic_callback=self.ros_diagnostics_pub.publish,
+            diagnostics_frequency=default_tree_diagnostics_frequency_hz,
             show_traceback_on_exception=show_traceback_on_exception,
         )
 

ros_bt_py/src/ros_bt_py/nodes/action.py

@@ -166,27 +166,33 @@ def _do_tick(self):
 
                 return NodeMsg.FAILED
 
+        if current_state is GoalStatus.SUCCEEDED:
+            result = self._ac.get_result()
+            if self._ac.get_result() is None:
+                return NodeMsg.RUNNING
+            self.outputs["result"] = result
+            # cancel goal to be sure, then stop tracking it so get_state()
+            # returns LOST again
+            self._active_goal = None
+
+            # Fail if final goal status was not SUCCEEDED
+            self.logerr("Succeeding")
+            return NodeMsg.SUCCEEDED
+
         if current_state in [
             GoalStatus.PREEMPTED,
-            GoalStatus.SUCCEEDED,
             GoalStatus.ABORTED,
             GoalStatus.REJECTED,
             GoalStatus.RECALLED,
             GoalStatus.LOST,
         ]:
-            # we're done, one way or the other
-            self.outputs["result"] = self._ac.get_result()
             # cancel goal to be sure, then stop tracking it so get_state()
             # returns LOST again
             self._ac.cancel_goal()
             self._ac.stop_tracking_goal()
             self._active_goal = None
 
-            # Fail if final goal status was not SUCCEEDED
-            if current_state == GoalStatus.SUCCEEDED:
-                return NodeMsg.SUCCEEDED
-            else:
-                return NodeMsg.FAILED
+            return NodeMsg.FAILED
 
         return NodeMsg.RUNNING