selfpatch · mfaferek93 · May 9, 2026 · May 9, 2026 · May 9, 2026 · May 9, 2026
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -13,7 +13,12 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ros_distro: [humble, jazzy]
+        # Humble dropped from the matrix because ros-humble-ros2-medkit-*
+        # debs are not yet available on packages.ros.org despite the
+        # rosdistro entry. The fork targets the medkit integration path,
+        # so a CI build without those debs is not meaningful. Re-add when
+        # Humble debs land.
+        ros_distro: [jazzy]
 
     container:
       image: ros:${{ matrix.ros_distro }}-ros-base
@@ -26,6 +31,9 @@ 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"
 
     steps:
       - name: Checkout repository
@@ -63,6 +71,10 @@ jobs:
             ccache \
             ros-$ROS_DISTRO-topic-based-ros2-control \
             python3-colcon-common-extensions python3-rosdep git \
+            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* \
             ros-$ROS_DISTRO-moveit \
             ros-$ROS_DISTRO-ur \
             ros-$ROS_DISTRO-ur-moveit-config \

diff --git a/CHANGELOG.rst b/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)
 ------------------

diff --git a/README.md b/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).

diff --git a/docs/FAULT_CODES.md b/docs/FAULT_CODES.md
@@ -0,0 +1,97 @@
+# 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).
+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.
diff --git a/manymove_bringup/launch/lite_foundationpose_movegroup_fake_cpp_trees.launch.py b/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),

diff --git a/manymove_bringup/launch/lite_isaac_sim_movegroup_fake_cpp_trees.launch.py b/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),

diff --git a/manymove_cpp_trees/CMakeLists.txt b/manymove_cpp_trees/CMakeLists.txt
@@ -29,10 +29,11 @@ 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)
+find_package(ros2_medkit_fault_reporter REQUIRED)
+find_package(ros2_medkit_msgs         REQUIRED)
 
 #--------------------------------------------------------------------
 # Library with all BT helper nodes
@@ -57,7 +58,8 @@ 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
+  ros2_medkit_fault_reporter ros2_medkit_msgs
 )
 
 #--------------------------------------------------------------------
@@ -72,7 +74,8 @@ 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
+    ros2_medkit_fault_reporter ros2_medkit_msgs
   )
 endmacro()
 
@@ -127,7 +130,8 @@ 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
+  ros2_medkit_fault_reporter ros2_medkit_msgs
 )
 
 if(BUILD_TESTING)
@@ -148,7 +152,8 @@ 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
+        ros2_medkit_fault_reporter ros2_medkit_msgs)
     endif()
   endfunction()
 
@@ -178,6 +183,9 @@ if(BUILD_TESTING)
 
   manymove_cpp_trees_add_gtest(test_bt_integration
     test/test_bt_integration.cpp)
+
+  manymove_cpp_trees_add_gtest(test_fault_reporting
+    test/test_fault_reporting.cpp)
 endif()
 
 ament_package()
diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_gripper.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_gripper.hpp
@@ -43,13 +43,15 @@
 
 #include "control_msgs/action/gripper_command.hpp"
 
+#include "manymove_cpp_trees/fault_reporting.hpp"
+
 namespace manymove_cpp_trees
 {
 // =======================================================
 // GripperCommandAction
 // =======================================================
 
-class GripperCommandAction : public BT::StatefulActionNode
+class GripperCommandAction : public BT::StatefulActionNode, public FaultReporting
 {
 public:
   using GripperCommand = control_msgs::action::GripperCommand;
@@ -97,7 +99,7 @@ class GripperCommandAction : public BT::StatefulActionNode
 // =======================================================
 // GripperTrajAction
 // =======================================================
-class GripperTrajAction : public BT::StatefulActionNode
+class GripperTrajAction : public BT::StatefulActionNode, public FaultReporting
 {
 public:
   using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory;
@@ -134,7 +136,7 @@ class GripperTrajAction : public BT::StatefulActionNode
 // PublishJointStateAction
 // =======================================================
 
-class PublishJointStateAction : public BT::SyncActionNode
+class PublishJointStateAction : public BT::SyncActionNode, public FaultReporting
 {
 public:
   PublishJointStateAction(const std::string & name, const BT::NodeConfiguration & config);

diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_isaac.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_isaac.hpp
@@ -50,6 +50,8 @@
 
 #include "rcpputils/thread_safety_annotations.hpp"
 
+#include "manymove_cpp_trees/fault_reporting.hpp"
+
 namespace manymove_cpp_trees
 {
 
@@ -80,7 +82,7 @@ namespace manymove_cpp_trees
  * warnings).
  * - FAILS if the required blackboard keys are not provided or if the service returns an error.
  */
-class GetEntityPoseNode : public BT::StatefulActionNode
+class GetEntityPoseNode : public BT::StatefulActionNode, public FaultReporting
 {
 public:
   using GetEntityState = simulation_interfaces::srv::GetEntityState;
@@ -143,7 +145,7 @@ class GetEntityPoseNode : public BT::StatefulActionNode
  * warnings).
  * - FAILS if the required blackboard keys are missing, or the service returns an error.
  */
-class SetEntityPoseNode : public BT::StatefulActionNode
+class SetEntityPoseNode : public BT::StatefulActionNode, public FaultReporting
 {
 public:
   using SetEntityState = simulation_interfaces::srv::SetEntityState;
@@ -256,7 +258,7 @@ geometry_msgs::msg::Pose align_foundationpose_orientation(
  * - FAILS if no valid detection arrives within timeout, or if TF transform to alignment/planning
  * frames times out.
  */
-class FoundationPoseAlignmentNode : public BT::StatefulActionNode
+class FoundationPoseAlignmentNode : public BT::StatefulActionNode, public FaultReporting
 {
 public:
   using DetectionArray = vision_msgs::msg::Detection3DArray;

diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_logic.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_logic.hpp
@@ -43,6 +43,8 @@
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
+#include "manymove_cpp_trees/fault_reporting.hpp"
+
 
 namespace manymove_cpp_trees
 {
@@ -154,7 +156,7 @@ class SetKeyBoolValue : public BT::SyncActionNode
  *   - "timeout" (double) : seconds, 0 => infinite
  *   - "poll_rate" (double) : frequency in s
  */
-class WaitForKeyBool : public BT::StatefulActionNode
+class WaitForKeyBool : public BT::StatefulActionNode, public FaultReporting
 {
 public:
   WaitForKeyBool(const std::string & name, const BT::NodeConfiguration & config);
@@ -207,7 +209,7 @@ class WaitForKeyBool : public BT::StatefulActionNode
  *  OUTPUT PORTS
  *    - pose             (geometry_msgs::msg::Pose)  Resulting pose
  */
-class GetLinkPoseAction : public BT::SyncActionNode
+class GetLinkPoseAction : public BT::SyncActionNode, public FaultReporting
 {
 public:
   GetLinkPoseAction(const std::string & name, const BT::NodeConfiguration & cfg);