diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 34977c5..dd41b03 100644 --- 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 index 3098749..4fa3120 100644 --- 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 index e0625cf..2eddf6b 100644 --- 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 new file mode 100644 index 0000000..727a855 --- /dev/null +++ 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__` +- 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 index cc79051..ba8cad6 100644 --- 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 index b12ef92..bfe64f3 100644 --- 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 index 113f19c..98db41e 100644 --- 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 index 2a26b0e..25ad48f 100644 --- 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 index 9fce995..23e1c24 100644 --- 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 index 8d450f0..c2f1971 100644 --- 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 #include +#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); diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_objects.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_objects.hpp index 9ba0bb4..9d95e41 100644 --- a/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_objects.hpp +++ b/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_objects.hpp @@ -47,6 +47,7 @@ #include #include "manymove_cpp_trees/bt_converters.hpp" +#include "manymove_cpp_trees/fault_reporting.hpp" #include "manymove_cpp_trees/move.hpp" #include "manymove_msgs/action/check_robot_state.hpp" #include "manymove_msgs/action/get_input.hpp" @@ -60,7 +61,7 @@ namespace manymove_cpp_trees * @brief A Behavior Tree node that adds a collision object to the planning scene using the * AddCollisionObject action server. */ -class AddCollisionObjectAction : public BT::StatefulActionNode +class AddCollisionObjectAction : public BT::StatefulActionNode, public FaultReporting { public: using AddCollisionObject = manymove_msgs::action::AddCollisionObject; @@ -128,7 +129,7 @@ class AddCollisionObjectAction : public BT::StatefulActionNode * @brief A Behavior Tree node that removes a collision object from the planning scene using the * RemoveCollisionObject action server. */ -class RemoveCollisionObjectAction : public BT::StatefulActionNode +class RemoveCollisionObjectAction : public BT::StatefulActionNode, public FaultReporting { public: using RemoveCollisionObject = manymove_msgs::action::RemoveCollisionObject; @@ -190,7 +191,7 @@ class RemoveCollisionObjectAction : public BT::StatefulActionNode * @brief A Behavior Tree node that attaches or detaches a collision object to/from a robot link * using the AttachDetachObject action server. */ -class AttachDetachObjectAction : public BT::StatefulActionNode +class AttachDetachObjectAction : public BT::StatefulActionNode, public FaultReporting { public: using AttachDetachObject = manymove_msgs::action::AttachDetachObject; @@ -260,7 +261,7 @@ class AttachDetachObjectAction : public BT::StatefulActionNode * @brief A Behavior Tree node that checks if a collision object exists and whether it's attached * using the CheckObjectExists action server. */ -class CheckObjectExistsAction : public BT::StatefulActionNode +class CheckObjectExistsAction : public BT::StatefulActionNode, public FaultReporting { public: using CheckObjectExists = manymove_msgs::action::CheckObjectExists; @@ -327,7 +328,7 @@ class CheckObjectExistsAction : public BT::StatefulActionNode * @brief A Behavior Tree node that retrieves and modifies the pose of a collision object using the * GetObjectPose action server. */ -class GetObjectPoseAction : public BT::StatefulActionNode +class GetObjectPoseAction : public BT::StatefulActionNode, public FaultReporting { public: using GetObjectPose = manymove_msgs::action::GetObjectPose; @@ -409,7 +410,7 @@ class GetObjectPoseAction : public BT::StatefulActionNode * - "is_attached" (bool, output) : Whether the object was attached. * - "link_name" (string, output) : If attached, which link. */ -class WaitForObjectAction : public BT::StatefulActionNode +class WaitForObjectAction : public BT::StatefulActionNode, public FaultReporting { public: using CheckObjectExists = manymove_msgs::action::CheckObjectExists; diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_planner.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_planner.hpp index 21ee99a..4bff3e0 100644 --- a/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_planner.hpp +++ b/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_planner.hpp @@ -47,6 +47,7 @@ #include #include +#include "manymove_cpp_trees/fault_reporting.hpp" #include "manymove_cpp_trees/move.hpp" #include "manymove_msgs/action/check_robot_state.hpp" #include "manymove_msgs/action/get_input.hpp" @@ -56,7 +57,7 @@ namespace manymove_cpp_trees { -class MoveManipulatorAction : public BT::StatefulActionNode +class MoveManipulatorAction : public BT::StatefulActionNode, public FaultReporting { public: using MoveManipulator = manymove_msgs::action::MoveManipulator; @@ -113,7 +114,7 @@ class MoveManipulatorAction : public BT::StatefulActionNode * It takes a comma-separated list of move_ids and for each, sets 'trajectory_{id}' to empty * and 'validity_{id}' to false in the blackboard. */ -class ResetTrajectories : public BT::SyncActionNode +class ResetTrajectories : public BT::SyncActionNode, public FaultReporting { public: /** diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_signals.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_signals.hpp index a81e61d..a305ada 100644 --- a/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_signals.hpp +++ b/manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_signals.hpp @@ -39,6 +39,7 @@ #include #include +#include "manymove_cpp_trees/fault_reporting.hpp" #include "manymove_cpp_trees/move.hpp" #include "manymove_msgs/action/add_collision_object.hpp" #include "manymove_msgs/action/attach_detach_object.hpp" @@ -59,7 +60,7 @@ namespace manymove_cpp_trees * @class SetOutputAction * @brief Sends a goal to the "set_output" action server (manymove_msgs::action::SetOutput). */ -class SetOutputAction : public BT::StatefulActionNode +class SetOutputAction : public BT::StatefulActionNode, public FaultReporting { public: using SetOutput = manymove_msgs::action::SetOutput; @@ -106,7 +107,7 @@ class SetOutputAction : public BT::StatefulActionNode * @class GetInputAction * @brief Reads a digital input from "get_input" action server (manymove_msgs::action::GetInput). */ -class GetInputAction : public BT::StatefulActionNode +class GetInputAction : public BT::StatefulActionNode, public FaultReporting { public: using GetInput = manymove_msgs::action::GetInput; @@ -152,7 +153,7 @@ class GetInputAction : public BT::StatefulActionNode * @class CheckRobotStateAction * @brief Check the robot's current state from "check_robot_state" action. */ -class CheckRobotStateAction : public BT::StatefulActionNode +class CheckRobotStateAction : public BT::StatefulActionNode, public FaultReporting { public: using CheckRobotState = manymove_msgs::action::CheckRobotState; @@ -194,7 +195,7 @@ class CheckRobotStateAction : public BT::StatefulActionNode * @class ResetRobotStateAction * @brief Send a goal to "reset_robot_state" (manymove_msgs::action::ResetRobotState). */ -class ResetRobotStateAction : public BT::StatefulActionNode +class ResetRobotStateAction : public BT::StatefulActionNode, public FaultReporting { public: using ResetRobotState = manymove_msgs::action::ResetRobotState; @@ -257,7 +258,7 @@ class ResetRobotStateAction : public BT::StatefulActionNode * @brief Repeatedly calls the "get_input" action server to read a digital input * and checks if the read value == desired_value. ... */ -class WaitForInputAction : public BT::StatefulActionNode +class WaitForInputAction : public BT::StatefulActionNode, public FaultReporting { public: WaitForInputAction(const std::string & name, const BT::NodeConfiguration & config); diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp new file mode 100644 index 0000000..04fdea1 --- /dev/null +++ b/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp @@ -0,0 +1,97 @@ +// Copyright 2026 Selfpatch.ai +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Selfpatch.ai nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MANYMOVE_CPP_TREES__FAULT_CODES_HPP_ +#define MANYMOVE_CPP_TREES__FAULT_CODES_HPP_ + +// Single source of truth for fault codes emitted by manymove_cpp_trees BT +// nodes via ros2_medkit_fault_reporter. Convention: +// MANYMOVE__ +// Allowed character set (per ros2_medkit_msgs ReportFault.srv): [A-Z0-9_]. +// Maximum length: 64 characters. +// +// Each code is reported by exactly the failure path documented in the comment +// above it. See docs/FAULT_CODES.md for severity, recovery semantics, and +// trigger conditions. + +namespace manymove_cpp_trees::fault_codes +{ + +// -- Planner ---------------------------------------------------------------- +// Collision detected when entering a move (action_nodes_planner.cpp onStart). +inline constexpr char kPlannerCollisionDetected[] = "MANYMOVE_PLANNER_COLLISION_DETECTED"; +// One retry of a move sequence failed; the move may still succeed. +inline constexpr char kPlannerRetryAttempt[] = "MANYMOVE_PLANNER_RETRY_ATTEMPT"; +// All retries exhausted, motion aborted. +inline constexpr char kPlannerRetriesExhausted[] = "MANYMOVE_PLANNER_RETRIES_EXHAUSTED"; +// External e-stop / stop_execution flag triggered while motion was running. +inline constexpr char kPlannerEstopTriggered[] = "MANYMOVE_PLANNER_ESTOP_TRIGGERED"; + +// -- Object manager --------------------------------------------------------- +// AddCollisionObject action returned failure (planning scene reject). +inline constexpr char kObjectAddFailed[] = "MANYMOVE_OBJECT_ADD_FAILED"; +// RemoveCollisionObject action returned failure. +inline constexpr char kObjectRemoveFailed[] = "MANYMOVE_OBJECT_REMOVE_FAILED"; +// AttachDetachObject action returned failure (link not found, attach reject). +inline constexpr char kObjectAttachFailed[] = "MANYMOVE_OBJECT_ATTACH_FAILED"; +// GetObjectPose action returned failure or the object is not in the scene. +inline constexpr char kObjectGetPoseFailed[] = "MANYMOVE_OBJECT_GET_POSE_FAILED"; +// WaitForObject elapsed without the expected object presence/absence. +inline constexpr char kObjectWaitTimeout[] = "MANYMOVE_OBJECT_WAIT_TIMEOUT"; + +// -- Signals (gripper, IO, robot state) ------------------------------------- +// SetOutputAction returned a failed result from the action server. +inline constexpr char kSignalSetOutputFailed[] = "MANYMOVE_SIGNAL_SET_OUTPUT_FAILED"; +// GetInputAction returned a failed result from the action server. +inline constexpr char kSignalGetInputFailed[] = "MANYMOVE_SIGNAL_GET_INPUT_FAILED"; +// WaitForInputAction elapsed without observing the desired input value. +inline constexpr char kSignalWaitInputTimeout[] = "MANYMOVE_SIGNAL_WAIT_INPUT_TIMEOUT"; +// CheckRobotStateAction reported the robot is not ready (err / mode mismatch). +inline constexpr char kRobotNotReady[] = "MANYMOVE_ROBOT_NOT_READY"; +// ResetRobotStateAction failed (controller unload / state reset / load step). +inline constexpr char kRobotResetFailed[] = "MANYMOVE_ROBOT_RESET_FAILED"; + +// -- Logic / TF (action-side, not condition checks) ------------------------- +// GetLinkPoseAction tf2 lookup raised TransformException. +inline constexpr char kTfLookupFailed[] = "MANYMOVE_TF_LOOKUP_FAILED"; +// WaitForKeyBool elapsed without observing the expected blackboard value. +inline constexpr char kWaitKeyTimeout[] = "MANYMOVE_WAIT_KEY_TIMEOUT"; + +// -- Gripper ---------------------------------------------------------------- +// GripperCommandAction goal aborted, canceled, or rejected. +inline constexpr char kGripperCommandFailed[] = "MANYMOVE_GRIPPER_COMMAND_FAILED"; +// GripperTrajAction trajectory execution failed. +inline constexpr char kGripperTrajFailed[] = "MANYMOVE_GRIPPER_TRAJ_FAILED"; + +// -- Isaac Sim integration -------------------------------------------------- +// FoundationPoseAlignmentNode could not lock onto a valid pose in time. +inline constexpr char kIsaacFoundationPoseFailed[] = "MANYMOVE_ISAAC_FOUNDATION_POSE_FAILED"; + +} // namespace manymove_cpp_trees::fault_codes + +#endif // MANYMOVE_CPP_TREES__FAULT_CODES_HPP_ diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/fault_reporting.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/fault_reporting.hpp new file mode 100644 index 0000000..807fe33 --- /dev/null +++ b/manymove_cpp_trees/include/manymove_cpp_trees/fault_reporting.hpp @@ -0,0 +1,110 @@ +// Copyright 2026 Selfpatch.ai +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Selfpatch.ai nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MANYMOVE_CPP_TREES__FAULT_REPORTING_HPP_ +#define MANYMOVE_CPP_TREES__FAULT_REPORTING_HPP_ + +#include + +#include +#include + +#include +#include + +namespace manymove_cpp_trees +{ + +// Blackboard key under which bt_client_*.cpp installs the shared FaultReporter +// for every BT node in the tree to consume. See installFaultReporter() in +// main_imports_helper.hpp. +inline constexpr char kFaultReporterBlackboardKey[] = "fault_reporter"; + +// Severity aliases that mirror ros2_medkit_msgs::msg::Fault::SEVERITY_*. Kept +// here as a header-local convenience so call sites do not have to spell out +// the verbose qualified constants on every reportFault() invocation. +inline constexpr uint8_t kSeverityInfo = ros2_medkit_msgs::msg::Fault::SEVERITY_INFO; +inline constexpr uint8_t kSeverityWarn = ros2_medkit_msgs::msg::Fault::SEVERITY_WARN; +inline constexpr uint8_t kSeverityError = ros2_medkit_msgs::msg::Fault::SEVERITY_ERROR; +inline constexpr uint8_t kSeverityCritical = ros2_medkit_msgs::msg::Fault::SEVERITY_CRITICAL; + +// Capability class that gives a BT action node one-line access to the +// process-wide FaultReporter installed on the blackboard. Inherited as a +// second base alongside BT::SyncActionNode / BT::StatefulActionNode by every +// BT node that reports faults; see fault_codes.hpp for the catalogue and +// docs/FAULT_CODES.md for which sites use which codes. +// +// Condition / decorator nodes (BT::ConditionNode, BT::DecoratorNode) MUST NOT +// inherit this: their FAILURE return is normal control flow, not a fault, +// and instrumenting them would flood the FaultManager. +// +// Lifetime: keeps a shared_ptr to the reporter so it is safe to use from any +// callback running on the same node executor. The reporter itself is owned +// by the bt_client_*.cpp main(); destruction order is fine because the +// blackboard outlives every BT node that pulled the pointer in its ctor. +class FaultReporting +{ +public: + // No virtual functions intentionally: this is a non-polymorphic capability + // class and we do not want any cost from a vtable in BT node base classes. + FaultReporting() = default; + +protected: + explicit FaultReporting(const BT::Blackboard::Ptr & blackboard) + { + if (blackboard) { + blackboard->get(kFaultReporterBlackboardKey, reporter_); + } + } + + // Report a FAILED event for the given fault_code. No-op when the reporter + // is not installed on the blackboard (e.g. unit tests that build a tree + // without bt_client_*.cpp scaffolding). + void reportFault(const char * fault_code, uint8_t severity, const std::string & description) const + { + if (reporter_) { + reporter_->report(fault_code, severity, description); + } + } + + // Report a PASSED event for healing/threshold reset. Pair with reportFault() + // when a previously reported transient condition (e.g. retry attempt) has + // cleared in the success branch of the same BT node. + void reportFaultPassed(const char * fault_code) const + { + if (reporter_) { + reporter_->report_passed(fault_code); + } + } + + std::shared_ptr reporter_; +}; + +} // namespace manymove_cpp_trees + +#endif // MANYMOVE_CPP_TREES__FAULT_REPORTING_HPP_ diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/main_imports_helper.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/main_imports_helper.hpp index facf2c9..ea13a34 100644 --- a/manymove_cpp_trees/include/manymove_cpp_trees/main_imports_helper.hpp +++ b/manymove_cpp_trees/include/manymove_cpp_trees/main_imports_helper.hpp @@ -34,6 +34,7 @@ #include #include +#include #include #include @@ -44,6 +45,7 @@ #include "manymove_cpp_trees/action_nodes_planner.hpp" #include "manymove_cpp_trees/action_nodes_signals.hpp" #include "manymove_cpp_trees/bt_converters.hpp" +#include "manymove_cpp_trees/fault_reporting.hpp" #include "manymove_cpp_trees/hmi_service_node.hpp" #include "manymove_cpp_trees/move.hpp" #include "manymove_cpp_trees/object.hpp" @@ -94,4 +96,25 @@ using manymove_cpp_trees::buildCheckPoseDistanceXML; using manymove_cpp_trees::mainTreeWrapperXML; using manymove_cpp_trees::registerAllNodeTypes; +namespace manymove_cpp_trees +{ + +// Construct a process-wide FaultReporter and stash it on the blackboard so +// every FaultReporting-equipped BT node can fetch it in its constructor. +// Call this once per bt_client_*.cpp main(), right after the canonical +// `blackboard->set("node", node)` line. +// +// source_id defaults to the node's fully-qualified name, which matches the +// SOVD apps[].ros_binding linkage used by the medkit gateway. +inline void installFaultReporter(BT::Blackboard::Ptr blackboard, rclcpp::Node::SharedPtr node) +{ + auto reporter = std::make_shared( + node, node->get_fully_qualified_name()); + blackboard->set(manymove_cpp_trees::kFaultReporterBlackboardKey, reporter); +} + +} // namespace manymove_cpp_trees + +using manymove_cpp_trees::installFaultReporter; + #endif // MANYMOVE_CPP_TREES__MAIN_IMPORTS_HELPER_HPP_ diff --git a/manymove_cpp_trees/package.xml b/manymove_cpp_trees/package.xml index f369e70..9c2d069 100644 --- a/manymove_cpp_trees/package.xml +++ b/manymove_cpp_trees/package.xml @@ -24,10 +24,11 @@ manymove_msgs control_msgs std_msgs - topic_based_ros2_control simulation_interfaces vision_msgs rcpputils + ros2_medkit_fault_reporter + ros2_medkit_msgs ament_lint_auto ament_lint_common diff --git a/manymove_cpp_trees/src/action_nodes_gripper.cpp b/manymove_cpp_trees/src/action_nodes_gripper.cpp index a36fe90..a5f38fe 100644 --- a/manymove_cpp_trees/src/action_nodes_gripper.cpp +++ b/manymove_cpp_trees/src/action_nodes_gripper.cpp @@ -31,6 +31,7 @@ #include #include "behaviortree_cpp_v3/behavior_tree.h" +#include "manymove_cpp_trees/fault_codes.hpp" using namespace std::chrono_literals; @@ -42,7 +43,7 @@ namespace manymove_cpp_trees // ------------------------------------------------- GripperCommandAction::GripperCommandAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), result_received_(false), server_ready_(false), @@ -100,6 +101,9 @@ BT::NodeStatus GripperCommandAction::onRunning() "GripperCommandAction: timed out waiting for action server '%s'.", action_server_name_.c_str()); waiting_for_server_ = false; + reportFault( + fault_codes::kGripperCommandFailed, kSeverityError, + "GripperCommand server '" + action_server_name_ + "' not available"); return BT::NodeStatus::FAILURE; } @@ -152,8 +156,14 @@ BT::NodeStatus GripperCommandAction::onRunning() double pos = action_result_.position; setOutput("current_position", pos); - // Return SUCCESS if truly reached the goal, else FAILURE - return success ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; + if (success) { + return BT::NodeStatus::SUCCESS; + } + reportFault( + fault_codes::kGripperCommandFailed, kSeverityError, + "GripperCommand goal not reached (reached_goal=false, stalled=false), final position=" + + std::to_string(pos)); + return BT::NodeStatus::FAILURE; } void GripperCommandAction::onHalted() @@ -201,7 +211,8 @@ void GripperCommandAction::feedbackCallback( // ------------------------------------------------- GripperTrajAction::GripperTrajAction(const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), goal_sent_(false), result_received_(false), success_(false) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), + result_received_(false), success_(false) { // Grab the node handle from blackboard if (!config.blackboard) { @@ -226,6 +237,9 @@ BT::NodeStatus GripperTrajAction::onStart() // Wait a few seconds for the action server if (!action_client_->wait_for_action_server(5s)) { RCLCPP_ERROR(node_->get_logger(), "GripperTrajAction: server not available!"); + reportFault( + fault_codes::kGripperTrajFailed, kSeverityError, + "GripperTraj action server not available within 5s"); return BT::NodeStatus::FAILURE; } @@ -270,15 +284,23 @@ BT::NodeStatus GripperTrajAction::onStart() BT::NodeStatus GripperTrajAction::onRunning() { if (!goal_sent_) { - return BT::NodeStatus::FAILURE; + // onRunning reached before onStart actually dispatched a goal: BT + // framework misuse, not an operational fault (per the + // programmer-error-throws / operational-fault-reports policy). + throw BT::RuntimeError("GripperTrajAction::onRunning called before goal was sent"); } if (!result_received_) { return BT::NodeStatus::RUNNING; } - // If result was received, return SUCCESS if success_ is true, else FAILURE - return success_ ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; + if (success_) { + return BT::NodeStatus::SUCCESS; + } + reportFault( + fault_codes::kGripperTrajFailed, kSeverityError, + "GripperTraj trajectory execution failed (action result code != SUCCEEDED)"); + return BT::NodeStatus::FAILURE; } void GripperTrajAction::onHalted() @@ -318,7 +340,7 @@ void GripperTrajAction::resultCallback( PublishJointStateAction::PublishJointStateAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::SyncActionNode(name, config) +: BT::SyncActionNode(name, config), FaultReporting(config.blackboard) { if (!config.blackboard || !config.blackboard->get("node", node_)) { throw BT::RuntimeError("PublishJointStateAction: 'node' not found in blackboard."); diff --git a/manymove_cpp_trees/src/action_nodes_isaac.cpp b/manymove_cpp_trees/src/action_nodes_isaac.cpp index 6056896..88c6a36 100644 --- a/manymove_cpp_trees/src/action_nodes_isaac.cpp +++ b/manymove_cpp_trees/src/action_nodes_isaac.cpp @@ -44,6 +44,7 @@ #include #include "manymove_cpp_trees/bt_converters.hpp" +#include "manymove_cpp_trees/fault_codes.hpp" #include @@ -60,7 +61,7 @@ namespace manymove_cpp_trees // GetEntityPoseNode // ====================================================================== GetEntityPoseNode::GetEntityPoseNode(const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard) { if (!config.blackboard || !config.blackboard->get("node", node_) || !node_) { throw BT::RuntimeError("GetEntityPoseNode: missing 'node' in blackboard"); @@ -162,7 +163,7 @@ void GetEntityPoseNode::onHalted() // SetEntityPoseNode // ====================================================================== SetEntityPoseNode::SetEntityPoseNode(const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard) { if (!config.blackboard || !config.blackboard->get("node", node_) || !node_) { throw BT::RuntimeError("SetEntityPoseNode: missing 'node' in blackboard"); @@ -392,7 +393,7 @@ geometry_msgs::msg::Pose align_foundationpose_orientation( FoundationPoseAlignmentNode::FoundationPoseAlignmentNode( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard) { if (!config.blackboard || !config.blackboard->get("node", node_) || !node_) { throw BT::RuntimeError("FoundationPoseAlignmentNode: missing 'node' in blackboard"); @@ -535,6 +536,10 @@ BT::NodeStatus FoundationPoseAlignmentNode::onRunning() RCLCPP_WARN( node_->get_logger(), "[%s] Timed out waiting for detections on '%s'", name().c_str(), topic_snapshot.c_str()); + reportFault( + fault_codes::kIsaacFoundationPoseFailed, kSeverityError, + "FoundationPose: no detection messages received on '" + topic_snapshot + + "' within " + std::to_string(timeout_seconds_) + "s"); return BT::NodeStatus::FAILURE; } } @@ -555,6 +560,11 @@ BT::NodeStatus FoundationPoseAlignmentNode::onRunning() node_->get_logger(), "[%s] Timed out waiting for a valid detection (target_id='%s', min_score=%.3f)", name().c_str(), target_id_.c_str(), minimum_score_); + reportFault( + fault_codes::kIsaacFoundationPoseFailed, kSeverityError, + "FoundationPose: no detection passed filters (target_id='" + target_id_ + + "', min_score=" + std::to_string(minimum_score_) + ") within " + + std::to_string(timeout_seconds_) + "s"); return BT::NodeStatus::FAILURE; } } @@ -598,6 +608,10 @@ BT::NodeStatus FoundationPoseAlignmentNode::onRunning() RCLCPP_ERROR( node_->get_logger(), "[%s] Timed out waiting for TF transform to '%s'", name().c_str(), alignment_frame.c_str()); + reportFault( + fault_codes::kIsaacFoundationPoseFailed, kSeverityError, + "FoundationPose: TF transform to '" + alignment_frame + + "' timed out after " + std::to_string(timeout_seconds_) + "s"); return BT::NodeStatus::FAILURE; } } @@ -636,6 +650,10 @@ BT::NodeStatus FoundationPoseAlignmentNode::onRunning() RCLCPP_ERROR( node_->get_logger(), "[%s] Timed out waiting for TF transform to '%s'", name().c_str(), planning_frame_.c_str()); + reportFault( + fault_codes::kIsaacFoundationPoseFailed, kSeverityError, + "FoundationPose: TF transform to planning frame '" + planning_frame_ + + "' timed out after " + std::to_string(timeout_seconds_) + "s"); return BT::NodeStatus::FAILURE; } } diff --git a/manymove_cpp_trees/src/action_nodes_logic.cpp b/manymove_cpp_trees/src/action_nodes_logic.cpp index f93deed..6f690b2 100644 --- a/manymove_cpp_trees/src/action_nodes_logic.cpp +++ b/manymove_cpp_trees/src/action_nodes_logic.cpp @@ -30,6 +30,7 @@ #include +#include "manymove_cpp_trees/fault_codes.hpp" #include "manymove_cpp_trees/hmi_utils.hpp" namespace manymove_cpp_trees @@ -230,7 +231,7 @@ BT::NodeStatus SetKeyBoolValue::tick() // WaitForKeyBool // --------------------------------------------------------- WaitForKeyBool::WaitForKeyBool(const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), condition_met_(false) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), condition_met_(false) { // If you need access to the node for time, etc. if (!config.blackboard) { @@ -325,6 +326,8 @@ BT::NodeStatus WaitForKeyBool::onRunning() setHMIMessage(config().blackboard, prefix_, "", "grey"); condition_met_ = true; + // Pair: clear timeout filter once the key matches. + reportFaultPassed(fault_codes::kWaitKeyTimeout); return BT::NodeStatus::SUCCESS; } @@ -335,6 +338,11 @@ BT::NodeStatus WaitForKeyBool::onRunning() RCLCPP_WARN( node_->get_logger(), "[%s] Timeout after %.2f s => FAILURE. last_value='%s'", name().c_str(), elapsed, (actual_value ? "true" : "false")); + reportFault( + fault_codes::kWaitKeyTimeout, kSeverityWarn, + "WaitForKeyBool '" + key_ + "' timed out after " + std::to_string(elapsed) + + "s, expected=" + (expected_value_ ? "true" : "false") + + ", last=" + (actual_value ? "true" : "false")); return BT::NodeStatus::FAILURE; } } @@ -365,7 +373,7 @@ constexpr double TF_TIMEOUT_SEC = 0.1; } // namespace GetLinkPoseAction::GetLinkPoseAction(const std::string & name, const BT::NodeConfiguration & cfg) -: BT::SyncActionNode(name, cfg) +: BT::SyncActionNode(name, cfg), FaultReporting(cfg.blackboard) { if (!cfg.blackboard || !cfg.blackboard->get("node", node_)) { throw BT::RuntimeError( @@ -407,6 +415,10 @@ BT::NodeStatus GetLinkPoseAction::tick() tf2::TimePointZero, tf2::durationFromSec(TF_TIMEOUT_SEC)); } catch (const tf2::TransformException & ex) { RCLCPP_ERROR(node_->get_logger(), "[%s] TF error: %s", name().c_str(), ex.what()); + reportFault( + fault_codes::kTfLookupFailed, kSeverityWarn, + std::string("TF lookup '") + link_name + "' -> '" + + (ref_frame.empty() ? "world" : ref_frame) + "': " + ex.what()); return BT::NodeStatus::FAILURE; } diff --git a/manymove_cpp_trees/src/action_nodes_objects.cpp b/manymove_cpp_trees/src/action_nodes_objects.cpp index 2f8544e..03c3e09 100644 --- a/manymove_cpp_trees/src/action_nodes_objects.cpp +++ b/manymove_cpp_trees/src/action_nodes_objects.cpp @@ -33,6 +33,7 @@ #include #include +#include "manymove_cpp_trees/fault_codes.hpp" #include "manymove_cpp_trees/hmi_utils.hpp" namespace manymove_cpp_trees @@ -41,7 +42,8 @@ namespace manymove_cpp_trees AddCollisionObjectAction::AddCollisionObjectAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), goal_sent_(false), result_received_(false) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), + result_received_(false) { static constexpr const char * kName = "AddCollisionObjectAction"; @@ -161,6 +163,9 @@ BT::NodeStatus AddCollisionObjectAction::onRunning() RCLCPP_ERROR( node_->get_logger(), "%s: Failed to add object '%s'. Message: %s", kName, object_id_.c_str(), action_result_.message.c_str()); + reportFault( + fault_codes::kObjectAddFailed, kSeverityError, + "add collision object '" + object_id_ + "' failed: " + action_result_.message); return BT::NodeStatus::FAILURE; } } @@ -234,7 +239,8 @@ void AddCollisionObjectAction::resultCallback( RemoveCollisionObjectAction::RemoveCollisionObjectAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), goal_sent_(false), result_received_(false) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), + result_received_(false) { static constexpr const char * kName = "RemoveCollisionObjectAction"; @@ -313,6 +319,9 @@ BT::NodeStatus RemoveCollisionObjectAction::onRunning() node_->get_logger(), "%s: Failed to remove object '%s'. Message: %s", kName, object_id_.c_str(), action_result_.message.c_str()); + reportFault( + fault_codes::kObjectRemoveFailed, kSeverityError, + "remove collision object '" + object_id_ + "' failed: " + action_result_.message); return BT::NodeStatus::FAILURE; } } @@ -388,7 +397,7 @@ void RemoveCollisionObjectAction::resultCallback( AttachDetachObjectAction::AttachDetachObjectAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), result_received_(false), attach_(true) // Default to attach @@ -495,6 +504,10 @@ BT::NodeStatus AttachDetachObjectAction::onRunning() "%s: Failed to %s object '%s' to link '%s'. Message: %s", kName, action.c_str(), object_id_.c_str(), link_name_.c_str(), action_result_.message.c_str()); + reportFault( + fault_codes::kObjectAttachFailed, kSeverityError, + action + " object '" + object_id_ + "' to link '" + link_name_ + "' failed: " + + action_result_.message); return BT::NodeStatus::FAILURE; } } @@ -569,7 +582,8 @@ void AttachDetachObjectAction::resultCallback( CheckObjectExistsAction::CheckObjectExistsAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), goal_sent_(false), result_received_(false) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), + result_received_(false) { static constexpr const char * kName = "CheckObjectExistsAction"; @@ -723,7 +737,8 @@ void CheckObjectExistsAction::resultCallback( GetObjectPoseAction::GetObjectPoseAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), goal_sent_(false), result_received_(false) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), + result_received_(false) { static constexpr const char * kName = "GetObjectPoseAction"; @@ -872,6 +887,9 @@ BT::NodeStatus GetObjectPoseAction::onRunning() node_->get_logger(), "%s: Failed to retrieve pose for object '%s'. Message: %s", kName, object_id_.c_str(), action_result_.message.c_str()); + reportFault( + fault_codes::kObjectGetPoseFailed, kSeverityError, + "GetObjectPose '" + object_id_ + "' failed: " + action_result_.message); return BT::NodeStatus::FAILURE; } } @@ -940,7 +958,7 @@ void GetObjectPoseAction::resultCallback( WaitForObjectAction::WaitForObjectAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), result_received_(false), last_exists_(false), @@ -1037,6 +1055,8 @@ BT::NodeStatus WaitForObjectAction::onRunning() node_->get_logger(), "%s: Condition met: object '%s' => exists=%s -> SUCCESS.", kName, object_id_.c_str(), (last_exists_ ? "true" : "false")); + // Pair: clear timeout filter once the wait condition is met. + reportFaultPassed(fault_codes::kObjectWaitTimeout); return BT::NodeStatus::SUCCESS; } @@ -1052,6 +1072,10 @@ BT::NodeStatus WaitForObjectAction::onRunning() node_->get_logger(), "%s: Timeout (%.1f s) reached for object '%s' -> FAILURE.", kName, timeout_, object_id_.c_str()); + reportFault( + fault_codes::kObjectWaitTimeout, kSeverityWarn, + "WaitForObject '" + object_id_ + "' timed out after " + + std::to_string(timeout_) + "s"); return BT::NodeStatus::FAILURE; } } @@ -1095,6 +1119,11 @@ void WaitForObjectAction::onHalted() RCLCPP_INFO(node_->get_logger(), "%s: Goal canceled.", kName); } + // Heal a previously raised wait-timeout: when the subtree is halted + // mid-wait, the timeout condition no longer holds. Without this, + // any earlier `kObjectWaitTimeout` would linger in FaultManager. + reportFaultPassed(fault_codes::kObjectWaitTimeout); + goal_sent_ = false; result_received_ = false; } diff --git a/manymove_cpp_trees/src/action_nodes_planner.cpp b/manymove_cpp_trees/src/action_nodes_planner.cpp index e3f86bf..74d84e2 100644 --- a/manymove_cpp_trees/src/action_nodes_planner.cpp +++ b/manymove_cpp_trees/src/action_nodes_planner.cpp @@ -33,6 +33,7 @@ #include #include +#include "manymove_cpp_trees/fault_codes.hpp" #include "manymove_cpp_trees/hmi_utils.hpp" namespace manymove_cpp_trees @@ -41,6 +42,7 @@ namespace manymove_cpp_trees MoveManipulatorAction::MoveManipulatorAction( const std::string & name, const BT::NodeConfiguration & config) : BT::StatefulActionNode(name, config), + FaultReporting(config.blackboard), goal_sent_(false), result_received_(false), max_tries_(-1), @@ -105,6 +107,9 @@ BT::NodeStatus MoveManipulatorAction::onStart() config().blackboard->set(robot_prefix_ + "collision_detected", false); config().blackboard->set(robot_prefix_ + "stop_execution", true); + reportFault( + fault_codes::kPlannerCollisionDetected, kSeverityError, + "collision detected on " + robot_prefix_ + " before motion start"); return BT::NodeStatus::FAILURE; } @@ -128,6 +133,9 @@ BT::NodeStatus MoveManipulatorAction::onStart() // HMI message setHMIMessage(config().blackboard, robot_prefix_, "WAITING FOR EXECUTION START", "yellow"); + reportFault( + fault_codes::kPlannerEstopTriggered, kSeverityCritical, + "stop_execution flag set on " + robot_prefix_ + " before motion start"); return BT::NodeStatus::FAILURE; } @@ -219,12 +227,21 @@ BT::NodeStatus MoveManipulatorAction::onRunning() setHMIMessage(config().blackboard, robot_prefix_, "", "grey"); RCLCPP_INFO(node_->get_logger(), "[MoveManipulatorAction] success => returning SUCCESS"); + // Heal the per-attempt soft fault if any retry was reported earlier. + reportFaultPassed(fault_codes::kPlannerRetryAttempt); return BT::NodeStatus::SUCCESS; } else { config().blackboard->set("trajectory_" + move_id_, trajectory_msgs::msg::JointTrajectory()); current_try_++; + // Every failed attempt is a soft fault; medkit's LocalFilter throttles + // these locally and only forwards to FaultManager once the threshold is + // crossed within its window. + reportFault( + fault_codes::kPlannerRetryAttempt, kSeverityWarn, + "attempt " + std::to_string(current_try_) + " failed: " + action_result_.message); + if (max_tries_ != -1 && current_try_ >= max_tries_) { RCLCPP_ERROR( node_->get_logger(), @@ -238,6 +255,10 @@ BT::NodeStatus MoveManipulatorAction::onRunning() setHMIMessage( config().blackboard, robot_prefix_, "MOTION FAILED: " + action_result_.message, "red"); + reportFault( + fault_codes::kPlannerRetriesExhausted, kSeverityError, + "motion failed after " + std::to_string(current_try_) + + " attempts: " + action_result_.message); return BT::NodeStatus::FAILURE; } else { RCLCPP_ERROR( @@ -330,7 +351,7 @@ void MoveManipulatorAction::feedbackCallback( } ResetTrajectories::ResetTrajectories(const std::string & name, const BT::NodeConfiguration & config) -: BT::SyncActionNode(name, config) +: BT::SyncActionNode(name, config), FaultReporting(config.blackboard) { // Obtain the ROS node from the blackboard if (!config.blackboard) { diff --git a/manymove_cpp_trees/src/action_nodes_signals.cpp b/manymove_cpp_trees/src/action_nodes_signals.cpp index 476d0a8..b76cd8d 100644 --- a/manymove_cpp_trees/src/action_nodes_signals.cpp +++ b/manymove_cpp_trees/src/action_nodes_signals.cpp @@ -33,6 +33,7 @@ #include #include +#include "manymove_cpp_trees/fault_codes.hpp" #include "manymove_cpp_trees/hmi_utils.hpp" namespace manymove_cpp_trees @@ -43,7 +44,8 @@ namespace manymove_cpp_trees // ------------------------------------------------------------------ SetOutputAction::SetOutputAction(const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), goal_sent_(false), result_received_(false) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), + result_received_(false) { // Obtain the ROS node from the blackboard if (!config.blackboard) { @@ -143,6 +145,10 @@ BT::NodeStatus SetOutputAction::onRunning() config().blackboard, prefix_, "SETTING OUTPUT " + std::to_string(ionum_) + " OF " + io_type_ + " FAILED!", "red"); + reportFault( + fault_codes::kSignalSetOutputFailed, kSeverityError, + "SetOutput " + io_type_ + "[" + std::to_string(ionum_) + "] failed: " + + action_result_.message); return BT::NodeStatus::FAILURE; } } @@ -189,7 +195,8 @@ void SetOutputAction::resultCallback(const GoalHandleSetOutput::WrappedResult & // ------------------------------------------------------------------ GetInputAction::GetInputAction(const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), goal_sent_(false), result_received_(false) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), + result_received_(false) { if (!config.blackboard) { throw BT::RuntimeError("GetInputAction: no blackboard provided."); @@ -285,6 +292,10 @@ BT::NodeStatus GetInputAction::onRunning() config().blackboard, prefix_, "READING INPUT " + std::to_string(ionum_) + " OF " + io_type_ + " FAILED!", "green"); + reportFault( + fault_codes::kSignalGetInputFailed, kSeverityError, + "GetInput " + io_type_ + "[" + std::to_string(ionum_) + "] failed: " + + action_result_.message); return BT::NodeStatus::FAILURE; } } @@ -329,7 +340,8 @@ void GetInputAction::resultCallback(const GoalHandleGetInput::WrappedResult & wr CheckRobotStateAction::CheckRobotStateAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), goal_sent_(false), result_received_(false) +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), + result_received_(false) { // Retrieve the ROS node from the blackboard if (!config.blackboard) { @@ -401,6 +413,8 @@ BT::NodeStatus CheckRobotStateAction::onRunning() RCLCPP_INFO( node_->get_logger(), "CheckRobotStateAction [%s]: Robot is READY. (mode=%d, state=%d)", name().c_str(), action_result_.mode, action_result_.state); + // Pair: clear NOT_READY filter once robot recovers. + reportFaultPassed(fault_codes::kRobotNotReady); return BT::NodeStatus::SUCCESS; } else { RCLCPP_WARN( @@ -408,6 +422,12 @@ BT::NodeStatus CheckRobotStateAction::onRunning() "CheckRobotStateAction [%s]: Robot is NOT ready => err=%d, mode=%d, state=%d. Msg=%s", name().c_str(), action_result_.err, action_result_.mode, action_result_.state, action_result_.message.c_str()); + reportFault( + fault_codes::kRobotNotReady, kSeverityCritical, + "robot not ready: err=" + std::to_string(action_result_.err) + + ", mode=" + std::to_string(action_result_.mode) + + ", state=" + std::to_string(action_result_.state) + + ", msg=" + action_result_.message); return BT::NodeStatus::FAILURE; } } @@ -464,7 +484,7 @@ void CheckRobotStateAction::resultCallback( ResetRobotStateAction::ResetRobotStateAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), result_received_(false), unload_traj_success_(false), @@ -637,6 +657,10 @@ void ResetRobotStateAction::goalResponseCallback( name().c_str()); action_result_.success = false; result_received_ = true; + reportFault( + fault_codes::kRobotResetFailed, kSeverityError, + std::string("ResetRobotStateAction [") + name() + + "]: reset_robot_state goal rejected by server"); } else { RCLCPP_INFO( node_->get_logger(), "ResetRobotStateAction [%s]: ResetRobotState Goal ACCEPTED by server.", @@ -652,6 +676,10 @@ void ResetRobotStateAction::resultCallback( } else { action_result_.success = false; action_result_.message = "ResetRobotState aborted or failed"; + reportFault( + fault_codes::kRobotResetFailed, kSeverityError, + std::string("ResetRobotStateAction [") + name() + + "]: reset_robot_state action aborted or failed"); } result_received_ = true; } @@ -664,6 +692,10 @@ void ResetRobotStateAction::goalResponseCallbackUnloadTraj( node_->get_logger(), "ResetRobotStateAction [%s]: UnloadTrajController Goal REJECTED by server.", name().c_str()); unload_traj_success_ = false; + reportFault( + fault_codes::kRobotResetFailed, kSeverityError, + std::string("ResetRobotStateAction [") + name() + + "]: unload_trajectory_controller goal rejected by server"); } else { RCLCPP_INFO( node_->get_logger(), @@ -676,8 +708,18 @@ void ResetRobotStateAction::resultCallbackUnloadTraj( { if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { unload_traj_success_ = wrapped_result.result->success; + if (!unload_traj_success_) { + reportFault( + fault_codes::kRobotResetFailed, kSeverityError, + std::string("ResetRobotStateAction [") + name() + + "]: unload_trajectory_controller reported success=false"); + } } else { unload_traj_success_ = false; + reportFault( + fault_codes::kRobotResetFailed, kSeverityError, + std::string("ResetRobotStateAction [") + name() + + "]: unload_trajectory_controller action aborted or failed"); } } @@ -689,6 +731,10 @@ void ResetRobotStateAction::goalResponseCallbackLoadTraj( node_->get_logger(), "ResetRobotStateAction [%s]: LoadTrajController Goal REJECTED by server.", name().c_str()); load_traj_success_ = false; + reportFault( + fault_codes::kRobotResetFailed, kSeverityError, + std::string("ResetRobotStateAction [") + name() + + "]: load_trajectory_controller goal rejected by server"); } else { RCLCPP_INFO( node_->get_logger(), @@ -701,14 +747,24 @@ void ResetRobotStateAction::resultCallbackLoadTraj( { if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) { load_traj_success_ = wrapped_result.result->success; + if (!load_traj_success_) { + reportFault( + fault_codes::kRobotResetFailed, kSeverityError, + std::string("ResetRobotStateAction [") + name() + + "]: load_trajectory_controller reported success=false"); + } } else { load_traj_success_ = false; + reportFault( + fault_codes::kRobotResetFailed, kSeverityError, + std::string("ResetRobotStateAction [") + name() + + "]: load_trajectory_controller action aborted or failed"); } } WaitForInputAction::WaitForInputAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), +: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), goal_sent_(false), result_received_(false), last_success_(false), @@ -762,6 +818,9 @@ BT::NodeStatus WaitForInputAction::onStart() RCLCPP_ERROR( node_->get_logger(), "[%s] server '%s' not available.", name().c_str(), server_name.c_str()); + reportFault( + fault_codes::kSignalGetInputFailed, kSeverityError, + "WaitForInput server '" + server_name + "' not available within 5s"); return BT::NodeStatus::FAILURE; } } @@ -804,6 +863,8 @@ BT::NodeStatus WaitForInputAction::onRunning() std::to_string(desired_value_), "green"); + // Pair: clear timeout filter once the desired value is observed. + reportFaultPassed(fault_codes::kSignalWaitInputTimeout); return BT::NodeStatus::SUCCESS; } @@ -819,6 +880,10 @@ BT::NodeStatus WaitForInputAction::onRunning() config().blackboard, prefix_, "WAIT INPUT " + std::to_string(ionum_) + " OF " + io_type_ + " TIMED OUT", "red"); + reportFault( + fault_codes::kSignalWaitInputTimeout, kSeverityWarn, + "WaitForInput " + io_type_ + "[" + std::to_string(ionum_) + "] timed out after " + + std::to_string(timeout_) + "s, last_value=" + std::to_string(last_value_)); return BT::NodeStatus::FAILURE; } } @@ -848,6 +913,11 @@ void WaitForInputAction::onHalted() if (goal_sent_ && !result_received_) { action_client_->async_cancel_all_goals(); } + // Heal a previously raised wait-timeout: when the subtree is halted + // mid-wait, the timeout condition no longer holds. Without this, + // any earlier `kSignalWaitInputTimeout` would linger in FaultManager + // until the next successful wait. + reportFaultPassed(fault_codes::kSignalWaitInputTimeout); goal_sent_ = false; result_received_ = false; } diff --git a/manymove_cpp_trees/src/bt_client.cpp b/manymove_cpp_trees/src/bt_client.cpp index 5dc5a69..65fb5ec 100644 --- a/manymove_cpp_trees/src/bt_client.cpp +++ b/manymove_cpp_trees/src/bt_client.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // ---------------------------------------------------------------------------- @@ -40,6 +40,7 @@ int main(int argc, char ** argv) // ---------------------------------------------------------------------------- auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/bt_client_app_dual.cpp b/manymove_cpp_trees/src/bt_client_app_dual.cpp index b6985ac..95d6738 100644 --- a/manymove_cpp_trees/src/bt_client_app_dual.cpp +++ b/manymove_cpp_trees/src/bt_client_app_dual.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client_app_dual"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // ---------------------------------------------------------------------------- @@ -41,6 +41,7 @@ int main(int argc, char ** argv) auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/bt_client_cumotion.cpp b/manymove_cpp_trees/src/bt_client_cumotion.cpp index d7e03c5..ed7895e 100644 --- a/manymove_cpp_trees/src/bt_client_cumotion.cpp +++ b/manymove_cpp_trees/src/bt_client_cumotion.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client_cumotion"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // ---------------------------------------------------------------------------- @@ -40,6 +40,7 @@ int main(int argc, char ** argv) // ---------------------------------------------------------------------------- auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/bt_client_dual.cpp b/manymove_cpp_trees/src/bt_client_dual.cpp index 968aab4..76819d4 100644 --- a/manymove_cpp_trees/src/bt_client_dual.cpp +++ b/manymove_cpp_trees/src/bt_client_dual.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client_dual"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // ---------------------------------------------------------------------------- @@ -41,6 +41,7 @@ int main(int argc, char ** argv) auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/bt_client_foundationpose.cpp b/manymove_cpp_trees/src/bt_client_foundationpose.cpp index 97ce295..205d380 100644 --- a/manymove_cpp_trees/src/bt_client_foundationpose.cpp +++ b/manymove_cpp_trees/src/bt_client_foundationpose.cpp @@ -53,6 +53,7 @@ int main(int argc, char ** argv) // Create a blackboard and set "node" auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); // Create the keys variable for HMI diff --git a/manymove_cpp_trees/src/bt_client_isaac.cpp b/manymove_cpp_trees/src/bt_client_isaac.cpp index 5ce648f..3d131aa 100644 --- a/manymove_cpp_trees/src/bt_client_isaac.cpp +++ b/manymove_cpp_trees/src/bt_client_isaac.cpp @@ -36,12 +36,13 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client_isaac"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // Create a blackboard and set "node" auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); // Create the keys variable for HMI diff --git a/manymove_cpp_trees/src/bt_client_mixed_pipelines.cpp b/manymove_cpp_trees/src/bt_client_mixed_pipelines.cpp index 5e7b174..4345988 100644 --- a/manymove_cpp_trees/src/bt_client_mixed_pipelines.cpp +++ b/manymove_cpp_trees/src/bt_client_mixed_pipelines.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client_mixed_pipelines"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // ---------------------------------------------------------------------------- @@ -40,6 +40,7 @@ int main(int argc, char ** argv) // ---------------------------------------------------------------------------- auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/bt_client_panda.cpp b/manymove_cpp_trees/src/bt_client_panda.cpp index 58457e3..c1c9c7e 100644 --- a/manymove_cpp_trees/src/bt_client_panda.cpp +++ b/manymove_cpp_trees/src/bt_client_panda.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client_panda"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // ---------------------------------------------------------------------------- @@ -40,6 +40,7 @@ int main(int argc, char ** argv) // ---------------------------------------------------------------------------- auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/bt_client_panda_cumotion.cpp b/manymove_cpp_trees/src/bt_client_panda_cumotion.cpp index 6ecb3c5..1602cde 100644 --- a/manymove_cpp_trees/src/bt_client_panda_cumotion.cpp +++ b/manymove_cpp_trees/src/bt_client_panda_cumotion.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client_panda_cumotion"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // ---------------------------------------------------------------------------- @@ -40,6 +40,7 @@ int main(int argc, char ** argv) // ---------------------------------------------------------------------------- auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/bt_client_uf850.cpp b/manymove_cpp_trees/src/bt_client_uf850.cpp index 0d96448..38fba05 100644 --- a/manymove_cpp_trees/src/bt_client_uf850.cpp +++ b/manymove_cpp_trees/src/bt_client_uf850.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client_uf850"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // ---------------------------------------------------------------------------- @@ -40,6 +40,7 @@ int main(int argc, char ** argv) // ---------------------------------------------------------------------------- auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/bt_client_ur.cpp b/manymove_cpp_trees/src/bt_client_ur.cpp index 2b5d730..de3b27d 100644 --- a/manymove_cpp_trees/src/bt_client_ur.cpp +++ b/manymove_cpp_trees/src/bt_client_ur.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client_ur"); RCLCPP_INFO(node->get_logger(), "BT Client Node with SignalColor started for UR."); // ---------------------------------------------------------------------------- @@ -41,6 +41,7 @@ int main(int argc, char ** argv) auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/bt_client_xarm7.cpp b/manymove_cpp_trees/src/bt_client_xarm7.cpp index b872630..8bf400e 100644 --- a/manymove_cpp_trees/src/bt_client_xarm7.cpp +++ b/manymove_cpp_trees/src/bt_client_xarm7.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("bt_client_xarm7"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // ---------------------------------------------------------------------------- @@ -40,6 +40,7 @@ int main(int argc, char ** argv) // ---------------------------------------------------------------------------- auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/collision_test.cpp b/manymove_cpp_trees/src/collision_test.cpp index e681705..3dacc69 100644 --- a/manymove_cpp_trees/src/collision_test.cpp +++ b/manymove_cpp_trees/src/collision_test.cpp @@ -32,7 +32,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("collision_test"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // ---------------------------------------------------------------------------- @@ -41,6 +41,7 @@ int main(int argc, char ** argv) auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); std::vector keys; diff --git a/manymove_cpp_trees/src/tree_helper.cpp b/manymove_cpp_trees/src/tree_helper.cpp index 6e7233c..1e1a14d 100644 --- a/manymove_cpp_trees/src/tree_helper.cpp +++ b/manymove_cpp_trees/src/tree_helper.cpp @@ -167,7 +167,7 @@ std::string buildMoveXML( // Check that the move's robot prefix is compatible if (!move.robot_prefix.empty() && (move.robot_prefix != robot_prefix)) { RCLCPP_ERROR( - rclcpp::get_logger("bt_client_node"), + rclcpp::get_logger("manymove_cpp_trees.tree_helper"), "buildMoveXML: Move has prefix=%s, but user gave robot_prefix=%s: INVALID MOVE.", move.robot_prefix.c_str(), robot_prefix.c_str()); return ""; @@ -179,7 +179,7 @@ std::string buildMoveXML( blackboard->set( "trajectory_" + std::to_string(this_move_id), trajectory_msgs::msg::JointTrajectory()); - RCLCPP_INFO(rclcpp::get_logger("bt_client_node"), "BB set: %s", key.c_str()); + RCLCPP_INFO(rclcpp::get_logger("manymove_cpp_trees.tree_helper"), "BB set: %s", key.c_str()); // Build a RetryPauseAbort node that wraps a single MoveManipulatorAction. // This node is expected to either execute an existing trajectory or trigger a re–plan. diff --git a/manymove_cpp_trees/src/tutorial_01.cpp b/manymove_cpp_trees/src/tutorial_01.cpp index dee3334..5ccf1ba 100644 --- a/manymove_cpp_trees/src/tutorial_01.cpp +++ b/manymove_cpp_trees/src/tutorial_01.cpp @@ -36,12 +36,13 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("tutorial_01"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // Create a blackboard and set "node" auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); // Create the keys variable for HMI diff --git a/manymove_cpp_trees/src/tutorial_01_complete.cpp b/manymove_cpp_trees/src/tutorial_01_complete.cpp index b274bb5..f7db2b0 100644 --- a/manymove_cpp_trees/src/tutorial_01_complete.cpp +++ b/manymove_cpp_trees/src/tutorial_01_complete.cpp @@ -35,12 +35,13 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("bt_client_node"); + auto node = rclcpp::Node::make_shared("tutorial_01_complete"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // Create a blackboard and set "node" auto blackboard = BT::Blackboard::create(); blackboard->set("node", node); + installFaultReporter(blackboard, node); RCLCPP_INFO(node->get_logger(), "Blackboard: set('node', )"); // Create the keys variable for HMI diff --git a/manymove_cpp_trees/test/fake_fault_manager.hpp b/manymove_cpp_trees/test/fake_fault_manager.hpp new file mode 100644 index 0000000..fc5cbe2 --- /dev/null +++ b/manymove_cpp_trees/test/fake_fault_manager.hpp @@ -0,0 +1,121 @@ +// Copyright 2026 Selfpatch.ai +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Selfpatch.ai nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef FAKE_FAULT_MANAGER_HPP_ +#define FAKE_FAULT_MANAGER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace manymove_cpp_trees::test +{ + +// Test fixture that stands in for the medkit FaultManager during gtest runs. +// It hosts the /fault_manager/report_fault service and records every accepted +// request so individual tests can assert which faults a BT node emitted. +// +// Mirrors the existing FakeXxxServer pattern in this test directory so tests +// stay readable for anyone already familiar with the codebase. +class FakeFaultManager +{ +public: + using ReportFault = ros2_medkit_msgs::srv::ReportFault; + + explicit FakeFaultManager(const rclcpp::Node::SharedPtr & node) + : node_(node) + { + service_ = node_->create_service( + "/fault_manager/report_fault", + [this]( + const std::shared_ptr request, + std::shared_ptr response) { + { + std::lock_guard lock(mutex_); + received_.push_back(*request); + } + response->accepted = true; + }); + } + + std::vector received() const + { + std::lock_guard lock(mutex_); + return received_; + } + + size_t count_of(const std::string & fault_code) const + { + std::lock_guard lock(mutex_); + size_t n = 0; + for (const auto & r : received_) { + if (r.fault_code == fault_code) { + ++n; + } + } + return n; + } + + // Spin-poll util: returns true once any received entry matches `fault_code`. + bool wait_for( + rclcpp::executors::SingleThreadedExecutor & exec, const std::string & fault_code, + std::chrono::milliseconds timeout) + { + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) { + if (count_of(fault_code) > 0) { + return true; + } + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + return false; + } + + void clear() + { + std::lock_guard lock(mutex_); + received_.clear(); + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Service::SharedPtr service_; + mutable std::mutex mutex_; + std::vector received_; +}; + +} // namespace manymove_cpp_trees::test + +#endif // FAKE_FAULT_MANAGER_HPP_ diff --git a/manymove_cpp_trees/test/test_fault_reporting.cpp b/manymove_cpp_trees/test/test_fault_reporting.cpp new file mode 100644 index 0000000..14ed257 --- /dev/null +++ b/manymove_cpp_trees/test/test_fault_reporting.cpp @@ -0,0 +1,174 @@ +// Copyright 2026 Selfpatch.ai +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the Selfpatch.ai nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include + +#include +#include +#include + +#include +#include + +#include "manymove_cpp_trees/fault_codes.hpp" +#include "manymove_cpp_trees/fault_reporting.hpp" +#include "manymove_cpp_trees/main_imports_helper.hpp" + +#include "fake_fault_manager.hpp" + +using manymove_cpp_trees::FaultReporting; +using manymove_cpp_trees::installFaultReporter; +using manymove_cpp_trees::test::FakeFaultManager; +using namespace std::chrono_literals; + +// Minimal subclass that exposes the protected reportFault helpers so tests +// can drive the mixin without spinning up a full BT.CPP factory. +class TestableReportingClass : public FaultReporting +{ +public: + explicit TestableReportingClass(const BT::Blackboard::Ptr & bb) + : FaultReporting(bb) {} + + using FaultReporting::reportFault; + using FaultReporting::reportFaultPassed; +}; + +class FaultReportingFixture : public ::testing::Test +{ +protected: + void SetUp() override + { + fault_manager_node_ = rclcpp::Node::make_shared("fake_fault_manager"); + fake_fm_ = std::make_unique(fault_manager_node_); + + bt_node_ = rclcpp::Node::make_shared("test_bt_client"); + blackboard_ = BT::Blackboard::create(); + blackboard_->set("node", bt_node_); + installFaultReporter(blackboard_, bt_node_); + + exec_.add_node(fault_manager_node_); + exec_.add_node(bt_node_); + } + + void TearDown() override + { + exec_.remove_node(bt_node_); + exec_.remove_node(fault_manager_node_); + fake_fm_.reset(); + bt_node_.reset(); + fault_manager_node_.reset(); + blackboard_.reset(); + } + + rclcpp::Node::SharedPtr fault_manager_node_; + rclcpp::Node::SharedPtr bt_node_; + std::unique_ptr fake_fm_; + BT::Blackboard::Ptr blackboard_; + rclcpp::executors::SingleThreadedExecutor exec_; +}; + +TEST_F(FaultReportingFixture, ReporterIsInstalledOnBlackboard) +{ + std::shared_ptr reporter; + ASSERT_TRUE(blackboard_->get(manymove_cpp_trees::kFaultReporterBlackboardKey, reporter)); + ASSERT_NE(reporter, nullptr); +} + +TEST_F(FaultReportingFixture, MixinForwardsErrorReportToFakeManager) +{ + TestableReportingClass node(blackboard_); + + node.reportFault( + manymove_cpp_trees::fault_codes::kPlannerCollisionDetected, + manymove_cpp_trees::kSeverityError, + "test collision"); + + ASSERT_TRUE(fake_fm_->wait_for( + exec_, manymove_cpp_trees::fault_codes::kPlannerCollisionDetected, 2s)); + + const auto received = fake_fm_->received(); + ASSERT_EQ(received.size(), 1u); + EXPECT_EQ(received.front().fault_code, + manymove_cpp_trees::fault_codes::kPlannerCollisionDetected); + EXPECT_EQ(received.front().severity, ros2_medkit_msgs::msg::Fault::SEVERITY_ERROR); + EXPECT_EQ(received.front().description, "test collision"); + EXPECT_EQ(received.front().source_id, std::string(bt_node_->get_fully_qualified_name())); +} + +TEST_F(FaultReportingFixture, ReportFaultPassedDeliversPassedEvent) +{ + TestableReportingClass node(blackboard_); + + // Send one FAILED first so the LocalFilter has something to clear. + node.reportFault( + manymove_cpp_trees::fault_codes::kPlannerRetryAttempt, + manymove_cpp_trees::kSeverityError, + "failure"); + ASSERT_TRUE(fake_fm_->wait_for( + exec_, manymove_cpp_trees::fault_codes::kPlannerRetryAttempt, 2s)); + fake_fm_->clear(); + + node.reportFaultPassed(manymove_cpp_trees::fault_codes::kPlannerRetryAttempt); + ASSERT_TRUE(fake_fm_->wait_for( + exec_, manymove_cpp_trees::fault_codes::kPlannerRetryAttempt, 2s)); + + const auto received = fake_fm_->received(); + ASSERT_EQ(received.size(), 1u); + EXPECT_EQ(received.front().event_type, ros2_medkit_msgs::srv::ReportFault::Request::EVENT_PASSED); +} + +TEST_F(FaultReportingFixture, MixinIsNoOpWhenReporterAbsentFromBlackboard) +{ + // Build a blackboard WITHOUT installFaultReporter -> reporter_ should be null + // and the mixin must silently no-op rather than crash. + auto empty_bb = BT::Blackboard::create(); + TestableReportingClass node(empty_bb); + + ASSERT_NO_THROW( + node.reportFault( + manymove_cpp_trees::fault_codes::kRobotNotReady, + manymove_cpp_trees::kSeverityCritical, "no reporter")); + ASSERT_NO_THROW( + node.reportFaultPassed(manymove_cpp_trees::fault_codes::kRobotNotReady)); + + // Nothing should reach the fake manager because the report was never sent. + exec_.spin_some(); + std::this_thread::sleep_for(200ms); + exec_.spin_some(); + EXPECT_EQ(fake_fm_->received().size(), 0u); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + const int rc = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return rc; +}