Feat/medkit integration

.github/workflows/ci.yml

@@ -14,6 +14,12 @@ jobs:
       fail-fast: false
       matrix:
         ros_distro: [humble, jazzy]
+        # with_medkit: ON exercises the medkit integration path (matches
+        # selfpatch fork's primary use case); OFF guards the upstream-friendly
+        # build for users who do not want the ros2_medkit overlay.
+        with_medkit: ["ON", "OFF"]
+
+    name: build-and-test (${{ matrix.ros_distro }}, medkit=${{ matrix.with_medkit }})
 
     container:
       image: ros:${{ matrix.ros_distro }}-ros-base
@@ -26,6 +32,10 @@ jobs:
       CCACHE_BASEDIR: /tmp/ros_ws
       CCACHE_COMPRESS: "1"
       CCACHE_MAXSIZE: 5G
+      # Pin medkit to the rosdistro-released version so an upstream bump
+      # does not silently break this CI. Update deliberately when bumping.
+      MEDKIT_VERSION: "0.4.0-1"
+      MANYMOVE_WITH_MEDKIT: ${{ matrix.with_medkit == 'ON' && '1' || '0' }}
 
     steps:
       - name: Checkout repository
@@ -36,8 +46,9 @@ jobs:
         uses: actions/cache@v4
         with:
           path: /root/.ccache
-          key: ccache-${{ matrix.ros_distro }}-${{ runner.os }}-v1
+          key: ccache-${{ matrix.ros_distro }}-medkit-${{ matrix.with_medkit }}-${{ runner.os }}-v1
           restore-keys: |
+            ccache-${{ matrix.ros_distro }}-medkit-${{ matrix.with_medkit }}-
             ccache-${{ matrix.ros_distro }}-
 
       - name: Add Isaac ROS apt repository (Jazzy)
@@ -55,6 +66,17 @@ jobs:
             > /etc/apt/sources.list.d/isaac-ros.list
           apt-get update
 
+      - name: Install medkit debs
+        if: matrix.with_medkit == 'ON'
+        run: |
+          set -e
+          apt-get update
+          apt-get install -y \
+            ros-$ROS_DISTRO-ros2-medkit-fault-reporter=$MEDKIT_VERSION* \
+            ros-$ROS_DISTRO-ros2-medkit-msgs=$MEDKIT_VERSION* \
+            ros-$ROS_DISTRO-ros2-medkit-fault-manager=$MEDKIT_VERSION* \
+            ros-$ROS_DISTRO-ros2-medkit-cmake=$MEDKIT_VERSION*
+
       - name: Install build tooling
         run: |
           set -e
@@ -114,7 +136,8 @@ jobs:
             --event-handlers console_cohesion+ \
             --cmake-args \
               -DCMAKE_C_COMPILER_LAUNCHER=ccache \
-              -DCMAKE_CXX_COMPILER_LAUNCHER=ccache
+              -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \
+              -DMANYMOVE_WITH_MEDKIT=${{ matrix.with_medkit }}
           # Show ccache status after build
           ccache -s || true
 

CHANGELOG.rst

@@ -9,7 +9,18 @@ high-level history.
 Forthcoming
 -----------
 
-- TBD.
+- ``manymove_cpp_trees``: rename every BT executable's ROS node to match the
+  binary name (``bt_client_xarm7``, ``bt_client_ur``, etc.) so source IDs are
+  distinct on the ROS graph instead of all sharing ``bt_client_node``.
+- ``manymove_cpp_trees``: hard dependency on ``ros2_medkit_fault_reporter``
+  and ``ros2_medkit_msgs``. BT action nodes inherit a small ``FaultReporting``
+  capability class and emit ``MANYMOVE_*`` fault codes on operational
+  failures (collision, retry, robot-not-ready, IO failures, TF lookups,
+  wait timeouts, FoundationPose timeouts). Catalogue in
+  ``docs/FAULT_CODES.md``. Vanilla manymove users should stay on
+  ``pastoriomarco/manymove``.
+- ``manymove_cpp_trees``: drop unused ``topic_based_ros2_control`` declaration
+  (still legitimately required by ``manymove_bringup``).
 
 0.3.2 (2025-12-03)
 ------------------

README.md

@@ -7,6 +7,17 @@
 
 ![ManyMove structure](media/manymove_example.gif)
 
+> **Selfpatch fork.** This repository is a fork of
+> [`pastoriomarco/manymove`](https://github.com/pastoriomarco/manymove)
+> instrumented with [`ros2_medkit_fault_reporter`](https://github.com/selfpatch/ros2_medkit)
+> so BT action node failures (collision, retry, robot-not-ready, IO/TF errors,
+> wait timeouts) are forwarded to the medkit `FaultManager` for diagnosis.
+> The medkit packages are a hard build-time dependency on this branch; install
+> them via apt (`ros-jazzy-ros2-medkit-fault-reporter`,
+> `ros-jazzy-ros2-medkit-msgs`) or build from source. See
+> [`docs/FAULT_CODES.md`](docs/FAULT_CODES.md) for the catalogue of emitted
+> codes.
+
 ## DISCLAIMER
 
 This software is released under the BSD-3-Clause license (see `LICENSE` for details).

docs/FAULT_CODES.md

@@ -0,0 +1,100 @@
+# manymove fault codes
+
+This is the catalogue of fault codes emitted by `manymove_cpp_trees` BT action
+nodes via [`ros2_medkit_fault_reporter`](https://github.com/selfpatch/ros2_medkit)
+when built with `MANYMOVE_WITH_MEDKIT=ON` (see the package README for the
+build instructions). With the option OFF the same `reportFault()` call sites
+compile to no-ops and nothing is published.
+Each entry below maps to one or more `reportFault()` call sites in the action
+node sources; the source-of-truth string constants live in
+`manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp`.
+
+## Conventions
+
+- Format: `MANYMOVE_<SUBSYSTEM>_<CONDITION>`
+- Character set: `[A-Z0-9_]`, max 64 characters (medkit `ReportFault.srv` limit)
+- Severity uses `ros2_medkit_msgs::msg::Fault::SEVERITY_*`:
+  - `INFO` (0), `WARN` (1), `ERROR` (2), `CRITICAL` (3)
+- Configuration / programmer-error FAILUREs (missing input ports, bad
+  blackboard keys) are intentionally NOT instrumented; only operational
+  faults are reported.
+- `BT::ConditionNode` and `BT::DecoratorNode` returns of FAILURE are normal
+  control flow and never produce fault reports.
+
+## Planner (`action_nodes_planner.cpp`)
+
+| Code | Severity | Trigger | Pair |
+|------|----------|---------|------|
+| `MANYMOVE_PLANNER_COLLISION_DETECTED` | ERROR | `MoveManipulatorAction::onStart` sees `collision_detected=true` before sending the goal. | - |
+| `MANYMOVE_PLANNER_ESTOP_TRIGGERED` | CRITICAL | `MoveManipulatorAction::onStart` sees `stop_execution=true`. | - |
+| `MANYMOVE_PLANNER_RETRY_ATTEMPT` | WARN | One attempt of `MoveManipulatorAction` failed; the move may still succeed on retry. Throttled locally by `LocalFilter`. | `reportFaultPassed` in the success branch |
+| `MANYMOVE_PLANNER_RETRIES_EXHAUSTED` | ERROR | `current_try_ >= max_tries_`; motion aborted. | - |
+
+## Object manager (`action_nodes_objects.cpp`)
+
+| Code | Severity | Trigger | Pair |
+|------|----------|---------|------|
+| `MANYMOVE_OBJECT_ADD_FAILED` | ERROR | `AddCollisionObjectAction` action result `success=false`. | - |
+| `MANYMOVE_OBJECT_REMOVE_FAILED` | ERROR | `RemoveCollisionObjectAction` action result `success=false`. | - |
+| `MANYMOVE_OBJECT_ATTACH_FAILED` | ERROR | `AttachDetachObjectAction` action result `success=false`. | - |
+| `MANYMOVE_OBJECT_GET_POSE_FAILED` | ERROR | `GetObjectPoseAction` action result `success=false`. | - |
+| `MANYMOVE_OBJECT_WAIT_TIMEOUT` | WARN | `WaitForObjectAction` elapsed without observing the expected presence/absence. | `reportFaultPassed` when the wait condition is met or on `onHalted` (subtree abandoned the wait). |
+
+`CheckObjectExistsAction` returning FAILURE for "object missing" is intentional
+control flow used by callers as a condition; it is not instrumented.
+
+## Signals (`action_nodes_signals.cpp`)
+
+| Code | Severity | Trigger | Pair |
+|------|----------|---------|------|
+| `MANYMOVE_SIGNAL_SET_OUTPUT_FAILED` | ERROR | `SetOutputAction` action result reports failure. | - |
+| `MANYMOVE_SIGNAL_GET_INPUT_FAILED` | ERROR | `GetInputAction` action result reports failure, or `WaitForInputAction` cannot connect to the action server within 5s. | - |
+| `MANYMOVE_SIGNAL_WAIT_INPUT_TIMEOUT` | WARN | `WaitForInputAction` timeout elapsed without observing the desired value. | `reportFaultPassed` on success or on `onHalted` (subtree abandoned the wait). |
+| `MANYMOVE_ROBOT_NOT_READY` | CRITICAL | `CheckRobotStateAction` reports `ready=false`. | `reportFaultPassed` once the robot reports `ready=true`. |
+| `MANYMOVE_ROBOT_RESET_FAILED` | ERROR | `ResetRobotStateAction` fails any of its three steps (unload trajectory controller, reset robot state, load trajectory controller); raised on goal rejection or non-SUCCEEDED result. | - |
+
+## Logic / TF (`action_nodes_logic.cpp`)
+
+| Code | Severity | Trigger | Pair |
+|------|----------|---------|------|
+| `MANYMOVE_TF_LOOKUP_FAILED` | WARN | `GetLinkPoseAction` tf2 `lookupTransform` raised `tf2::TransformException`. | - |
+| `MANYMOVE_WAIT_KEY_TIMEOUT` | WARN | `WaitForKeyBool` elapsed without the expected blackboard value. | `reportFaultPassed` once the key matches. |
+
+## Gripper (`action_nodes_gripper.cpp`)
+
+| Code | Severity | Trigger | Pair |
+|------|----------|---------|------|
+| `MANYMOVE_GRIPPER_COMMAND_FAILED` | ERROR | `GripperCommandAction` action server unavailable, or goal reported `reached_goal=false` and `stalled=false`. | - |
+| `MANYMOVE_GRIPPER_TRAJ_FAILED` | ERROR | `GripperTrajAction` action server unavailable or trajectory result code != `SUCCEEDED`. | - |
+
+## Isaac Sim (`action_nodes_isaac.cpp`)
+
+| Code | Severity | Trigger | Pair |
+|------|----------|---------|------|
+| `MANYMOVE_ISAAC_FOUNDATION_POSE_FAILED` | ERROR | `FoundationPoseAlignmentNode` timed out waiting for detections, no detection passed the `target_id`/`min_score` filter within the configured timeout, or TF transform of the detection pose to the alignment / planning frame timed out. | - |
+
+`GetEntityPoseNode`/`SetEntityPoseNode` service errors and Isaac TF timeouts
+are not yet instrumented; they will gain dedicated codes when the
+`manymove_industrial` demo wires them up.
+
+## Local filtering
+
+`ros2_medkit_fault_reporter::LocalFilter` debounces repeated reports of the
+same `fault_code` so soft-fault burst patterns (e.g. several
+`MANYMOVE_PLANNER_RETRY_ATTEMPT` in quick succession) do not flood the
+`FaultManager`. Defaults: `default_threshold=3`, `default_window_sec=10.0`,
+`bypass_severity=ERROR`. Configure per-bt_client via standard ROS parameters:
+
+```yaml
+bt_client_xarm7:
+  ros__parameters:
+    fault_reporter:
+      local_filtering:
+        enabled: true
+        default_threshold: 3
+        default_window_sec: 10.0
+        bypass_severity: 2
+```
+
+Pair `reportFaultPassed` with the matching `reportFault` on the success branch
+of any throttled code so the filter resets cleanly between cycles.

manymove_bringup/launch/lite_foundationpose_movegroup_fake_cpp_trees.launch.py

@@ -362,13 +362,13 @@ def launch_setup(context, *args, **kwargs):
         arguments=[
             '--ros-args',
             '--log-level',
-            'bt_client_node:=' + log_level.perform(context),
+            'bt_client_foundationpose:=' + log_level.perform(context),
             '--log-level',
             'rcl:=info',
             '--log-level',
             'rclcpp:=info',
             '--log-level',
-            'bt_client_node.rclcpp_action:=info',
+            'bt_client_foundationpose.rclcpp_action:=info',
         ],
         parameters=[
             {
@@ -506,7 +506,7 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 'log_level',
                 default_value='info',
-                description='Defines log level for bt_client_node',
+                description='Defines log level for bt_client_foundationpose',
             ),
             # OpaqueFunction to set up the node
             OpaqueFunction(function=launch_setup),

manymove_bringup/launch/lite_isaac_sim_movegroup_fake_cpp_trees.launch.py

@@ -362,13 +362,13 @@ def launch_setup(context, *args, **kwargs):
         arguments=[
             '--ros-args',
             '--log-level',
-            'bt_client_node:=' + log_level.perform(context),
+            'bt_client_isaac:=' + log_level.perform(context),
             '--log-level',
             'rcl:=info',
             '--log-level',
             'rclcpp:=info',
             '--log-level',
-            'bt_client_node.rclcpp_action:=info',
+            'bt_client_isaac.rclcpp_action:=info',
         ],
         parameters=[
             {
@@ -506,7 +506,7 @@ def generate_launch_description():
             DeclareLaunchArgument(
                 'log_level',
                 default_value='info',
-                description='Defines log level for bt_client_node',
+                description='Defines log level for bt_client_isaac',
             ),
             # OpaqueFunction to set up the node
             OpaqueFunction(function=launch_setup),

manymove_cpp_trees/CMakeLists.txt

@@ -11,6 +11,18 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wthread-safety)
 endif()
 
+#--------------------------------------------------------------------
+# Build options
+#--------------------------------------------------------------------
+# ros2_medkit fault reporting is off by default to keep the upstream
+# build free of extra deps. Turn it on (-DMANYMOVE_WITH_MEDKIT=ON plus
+# MANYMOVE_WITH_MEDKIT=1 in the env for rosdep) to pull in the medkit
+# overlay; with it off, the FaultReporting mixin and installFaultReporter()
+# compile to no-ops so action nodes build unchanged.
+option(MANYMOVE_WITH_MEDKIT
+  "Build with ros2_medkit fault reporting integration"
+  OFF)
+
 #--------------------------------------------------------------------
 # Dependencies
 #--------------------------------------------------------------------
@@ -29,11 +41,15 @@ find_package(std_srvs                 REQUIRED)
 find_package(manymove_msgs            REQUIRED)
 find_package(control_msgs             REQUIRED)
 find_package(std_msgs                 REQUIRED)
-find_package(topic_based_ros2_control REQUIRED)
 find_package(simulation_interfaces    REQUIRED)
 find_package(vision_msgs              REQUIRED)
 find_package(rcpputils                REQUIRED)
 
+if(MANYMOVE_WITH_MEDKIT)
+  find_package(ros2_medkit_fault_reporter REQUIRED)
+  find_package(ros2_medkit_msgs         REQUIRED)
+endif()
+
 #--------------------------------------------------------------------
 # Library with all BT helper nodes
 #--------------------------------------------------------------------
@@ -57,9 +73,18 @@ ament_target_dependencies(manymove_cpp_trees_lib
   rclcpp rclcpp_action behaviortree_cpp_v3
   manymove_planner manymove_object_manager
   trajectory_msgs geometry_msgs tf2 tf2_ros tf2_geometry_msgs std_srvs manymove_msgs control_msgs
-  std_msgs topic_based_ros2_control simulation_interfaces vision_msgs rcpputils
+  std_msgs simulation_interfaces vision_msgs rcpputils
 )
 
+if(MANYMOVE_WITH_MEDKIT)
+  ament_target_dependencies(manymove_cpp_trees_lib
+    ros2_medkit_fault_reporter ros2_medkit_msgs)
+  # PUBLIC so consumers (bt_client_* execs, tests) inherit the define and
+  # pick the medkit-enabled branches in fault_reporting.hpp /
+  # main_imports_helper.hpp without each target having to set it again.
+  target_compile_definitions(manymove_cpp_trees_lib PUBLIC MANYMOVE_WITH_MEDKIT)
+endif()
+
 #--------------------------------------------------------------------
 # Helper macro to create BT-client executables
 #--------------------------------------------------------------------
@@ -72,8 +97,13 @@ macro(bt_client_exec NAME)
     rclcpp rclcpp_action behaviortree_cpp_v3
     manymove_planner manymove_object_manager
     trajectory_msgs tf2 std_srvs manymove_msgs control_msgs
-    std_msgs topic_based_ros2_control simulation_interfaces rcpputils
+    std_msgs simulation_interfaces rcpputils
   )
+
+  if(MANYMOVE_WITH_MEDKIT)
+    ament_target_dependencies(${NAME}
+      ros2_medkit_fault_reporter ros2_medkit_msgs)
+  endif()
 endmacro()
 
 bt_client_exec(bt_client)
@@ -127,9 +157,13 @@ ament_export_dependencies(
   rclcpp rclcpp_action behaviortree_cpp_v3
   manymove_planner manymove_object_manager
   trajectory_msgs tf2 tf2_ros tf2_geometry_msgs std_srvs manymove_msgs control_msgs
-  std_msgs topic_based_ros2_control simulation_interfaces vision_msgs rcpputils
+  std_msgs simulation_interfaces vision_msgs rcpputils
 )
 
+if(MANYMOVE_WITH_MEDKIT)
+  ament_export_dependencies(ros2_medkit_fault_reporter ros2_medkit_msgs)
+endif()
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_uncrustify REQUIRED)
@@ -148,7 +182,11 @@ if(BUILD_TESTING)
         rclcpp rclcpp_action behaviortree_cpp_v3
         manymove_planner manymove_object_manager
         trajectory_msgs geometry_msgs tf2 tf2_ros tf2_geometry_msgs std_srvs manymove_msgs control_msgs
-        std_msgs topic_based_ros2_control simulation_interfaces vision_msgs rcpputils)
+        std_msgs simulation_interfaces vision_msgs rcpputils)
+      if(MANYMOVE_WITH_MEDKIT)
+        ament_target_dependencies(${target}
+          ros2_medkit_fault_reporter ros2_medkit_msgs)
+      endif()
     endif()
   endfunction()
 
@@ -178,6 +216,11 @@ if(BUILD_TESTING)
 
   manymove_cpp_trees_add_gtest(test_bt_integration
     test/test_bt_integration.cpp)
+
+  if(MANYMOVE_WITH_MEDKIT)
+    manymove_cpp_trees_add_gtest(test_fault_reporting
+      test/test_fault_reporting.cpp)
+  endif()
 endif()
 
 ament_package()

manymove_cpp_trees/README.md

@@ -42,6 +42,21 @@ Each executable spins the BT factory, registers the custom nodes, and runs the `
 - Control and runtime messaging: `rclcpp`, `rclcpp_action`, `control_msgs`, `std_msgs`, `std_srvs`, `topic_based_ros2_control`
 - Simulation and perception bridges: `simulation_interfaces`, `vision_msgs`
 
+## Optional: ros2_medkit fault reporting
+The package can emit structured fault events via
+[`ros2_medkit`](https://github.com/selfpatch/ros2_medkit) when built with
+`MANYMOVE_WITH_MEDKIT=ON`. With the option OFF (default) the action nodes
+build with no extra dependencies and every `reportFault()` call compiles
+to a no-op. To enable:
+
+```bash
+export MANYMOVE_WITH_MEDKIT=1                                # for rosdep
+rosdep install --from-paths src --ignore-src -y
+colcon build --cmake-args -DMANYMOVE_WITH_MEDKIT=ON          # for the build
+```
+
+See [`docs/FAULT_CODES.md`](../docs/FAULT_CODES.md) for the catalogue.
+
 ## Notes
 - Robot safety mechanisms (stop buttons, workspace supervision) must be handled externally.
 - Review the main README for project-wide disclaimers, contribution guidance, and licensing details.