From 9ee823580bd83eb5466a2b3fe81d228501bc15f7 Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 21:18:53 +0200 Subject: [PATCH 01/15] chore(cpp_trees): drop unused topic_based_ros2_control dependency Declared in package.xml and CMakeLists.txt but never #included anywhere in src/, include/ or test/. Build is green without it; manymove_bringup keeps the dep where it is actually used. --- manymove_cpp_trees/CMakeLists.txt | 9 ++++----- manymove_cpp_trees/package.xml | 1 - 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/manymove_cpp_trees/CMakeLists.txt b/manymove_cpp_trees/CMakeLists.txt index 113f19c..492eee4 100644 --- a/manymove_cpp_trees/CMakeLists.txt +++ b/manymove_cpp_trees/CMakeLists.txt @@ -29,7 +29,6 @@ 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) @@ -57,7 +56,7 @@ 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 ) #-------------------------------------------------------------------- @@ -72,7 +71,7 @@ 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 ) endmacro() @@ -127,7 +126,7 @@ ament_export_dependencies( rclcpp rclcpp_action behaviortree_cpp_v3 manymove_planner manymove_object_manager trajectory_msgs tf2 tf2_ros tf2_geometry_msgs std_srvs manymove_msgs control_msgs - std_msgs topic_based_ros2_control simulation_interfaces vision_msgs rcpputils + std_msgs simulation_interfaces vision_msgs rcpputils ) if(BUILD_TESTING) @@ -148,7 +147,7 @@ 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) endif() endfunction() diff --git a/manymove_cpp_trees/package.xml b/manymove_cpp_trees/package.xml index f369e70..0634b24 100644 --- a/manymove_cpp_trees/package.xml +++ b/manymove_cpp_trees/package.xml @@ -24,7 +24,6 @@ manymove_msgs control_msgs std_msgs - topic_based_ros2_control simulation_interfaces vision_msgs rcpputils From bb75fbde9d0f15ef670d219388f17e4a3da05560 Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 21:50:03 +0200 Subject: [PATCH 02/15] refactor(cpp_trees): give every BT executable a unique ROS node name All bt_client_*, collision_test and tutorials shared "bt_client_node", so source_id was indistinguishable across them. Each now matches its executable; the static logger in tree_helper.cpp and two log-level remaps in manymove_bringup launch files moved with them. --- .../lite_foundationpose_movegroup_fake_cpp_trees.launch.py | 6 +++--- .../lite_isaac_sim_movegroup_fake_cpp_trees.launch.py | 6 +++--- manymove_cpp_trees/src/bt_client.cpp | 2 +- manymove_cpp_trees/src/bt_client_app_dual.cpp | 2 +- manymove_cpp_trees/src/bt_client_cumotion.cpp | 2 +- manymove_cpp_trees/src/bt_client_dual.cpp | 2 +- manymove_cpp_trees/src/bt_client_isaac.cpp | 2 +- manymove_cpp_trees/src/bt_client_mixed_pipelines.cpp | 2 +- manymove_cpp_trees/src/bt_client_panda.cpp | 2 +- manymove_cpp_trees/src/bt_client_panda_cumotion.cpp | 2 +- manymove_cpp_trees/src/bt_client_uf850.cpp | 2 +- manymove_cpp_trees/src/bt_client_ur.cpp | 2 +- manymove_cpp_trees/src/bt_client_xarm7.cpp | 2 +- manymove_cpp_trees/src/collision_test.cpp | 2 +- manymove_cpp_trees/src/tree_helper.cpp | 4 ++-- manymove_cpp_trees/src/tutorial_01.cpp | 2 +- manymove_cpp_trees/src/tutorial_01_complete.cpp | 2 +- 17 files changed, 22 insertions(+), 22 deletions(-) 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/src/bt_client.cpp b/manymove_cpp_trees/src/bt_client.cpp index 5dc5a69..e0e1811 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)."); // ---------------------------------------------------------------------------- diff --git a/manymove_cpp_trees/src/bt_client_app_dual.cpp b/manymove_cpp_trees/src/bt_client_app_dual.cpp index b6985ac..5f0711c 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)."); // ---------------------------------------------------------------------------- diff --git a/manymove_cpp_trees/src/bt_client_cumotion.cpp b/manymove_cpp_trees/src/bt_client_cumotion.cpp index d7e03c5..536c889 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)."); // ---------------------------------------------------------------------------- diff --git a/manymove_cpp_trees/src/bt_client_dual.cpp b/manymove_cpp_trees/src/bt_client_dual.cpp index 968aab4..706a4f0 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)."); // ---------------------------------------------------------------------------- diff --git a/manymove_cpp_trees/src/bt_client_isaac.cpp b/manymove_cpp_trees/src/bt_client_isaac.cpp index 5ce648f..119283a 100644 --- a/manymove_cpp_trees/src/bt_client_isaac.cpp +++ b/manymove_cpp_trees/src/bt_client_isaac.cpp @@ -36,7 +36,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_isaac"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // Create a blackboard and set "node" diff --git a/manymove_cpp_trees/src/bt_client_mixed_pipelines.cpp b/manymove_cpp_trees/src/bt_client_mixed_pipelines.cpp index 5e7b174..224e01c 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)."); // ---------------------------------------------------------------------------- diff --git a/manymove_cpp_trees/src/bt_client_panda.cpp b/manymove_cpp_trees/src/bt_client_panda.cpp index 58457e3..4b1f49b 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)."); // ---------------------------------------------------------------------------- diff --git a/manymove_cpp_trees/src/bt_client_panda_cumotion.cpp b/manymove_cpp_trees/src/bt_client_panda_cumotion.cpp index 6ecb3c5..1d98ca1 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)."); // ---------------------------------------------------------------------------- diff --git a/manymove_cpp_trees/src/bt_client_uf850.cpp b/manymove_cpp_trees/src/bt_client_uf850.cpp index 0d96448..d683385 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)."); // ---------------------------------------------------------------------------- diff --git a/manymove_cpp_trees/src/bt_client_ur.cpp b/manymove_cpp_trees/src/bt_client_ur.cpp index 2b5d730..494aadb 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."); // ---------------------------------------------------------------------------- diff --git a/manymove_cpp_trees/src/bt_client_xarm7.cpp b/manymove_cpp_trees/src/bt_client_xarm7.cpp index b872630..4029a72 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)."); // ---------------------------------------------------------------------------- diff --git a/manymove_cpp_trees/src/collision_test.cpp b/manymove_cpp_trees/src/collision_test.cpp index e681705..5b7afc4 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)."); // ---------------------------------------------------------------------------- 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..c75ae15 100644 --- a/manymove_cpp_trees/src/tutorial_01.cpp +++ b/manymove_cpp_trees/src/tutorial_01.cpp @@ -36,7 +36,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("tutorial_01"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // Create a blackboard and set "node" diff --git a/manymove_cpp_trees/src/tutorial_01_complete.cpp b/manymove_cpp_trees/src/tutorial_01_complete.cpp index b274bb5..503d0c9 100644 --- a/manymove_cpp_trees/src/tutorial_01_complete.cpp +++ b/manymove_cpp_trees/src/tutorial_01_complete.cpp @@ -35,7 +35,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("tutorial_01_complete"); RCLCPP_INFO(node->get_logger(), "BT Client Node started (Purely Programmatic XML)."); // Create a blackboard and set "node" From eb3606a3ba9c415610ae7e503b83129d3d2c3b2f Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 21:56:01 +0200 Subject: [PATCH 03/15] build(cpp_trees): wire ros2_medkit_fault_reporter as a hard dependency No CMake option, no conditional : this fork exists for the medkit integration. If we ever upstream to pastoriomarco/manymove, put it behind a toggle so vanilla users do not pull medkit transitively. --- manymove_cpp_trees/CMakeLists.txt | 8 +++++++- manymove_cpp_trees/package.xml | 2 ++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/manymove_cpp_trees/CMakeLists.txt b/manymove_cpp_trees/CMakeLists.txt index 492eee4..f8c28b6 100644 --- a/manymove_cpp_trees/CMakeLists.txt +++ b/manymove_cpp_trees/CMakeLists.txt @@ -32,6 +32,8 @@ find_package(std_msgs 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,6 +59,7 @@ ament_target_dependencies(manymove_cpp_trees_lib manymove_planner manymove_object_manager trajectory_msgs geometry_msgs tf2 tf2_ros tf2_geometry_msgs std_srvs manymove_msgs control_msgs std_msgs simulation_interfaces vision_msgs rcpputils + ros2_medkit_fault_reporter ros2_medkit_msgs ) #-------------------------------------------------------------------- @@ -72,6 +75,7 @@ macro(bt_client_exec NAME) manymove_planner manymove_object_manager trajectory_msgs tf2 std_srvs manymove_msgs control_msgs std_msgs simulation_interfaces rcpputils + ros2_medkit_fault_reporter ros2_medkit_msgs ) endmacro() @@ -127,6 +131,7 @@ ament_export_dependencies( manymove_planner manymove_object_manager trajectory_msgs tf2 tf2_ros tf2_geometry_msgs std_srvs manymove_msgs control_msgs std_msgs simulation_interfaces vision_msgs rcpputils + ros2_medkit_fault_reporter ros2_medkit_msgs ) if(BUILD_TESTING) @@ -147,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 simulation_interfaces vision_msgs rcpputils) + std_msgs simulation_interfaces vision_msgs rcpputils + ros2_medkit_fault_reporter ros2_medkit_msgs) endif() endfunction() diff --git a/manymove_cpp_trees/package.xml b/manymove_cpp_trees/package.xml index 0634b24..9c2d069 100644 --- a/manymove_cpp_trees/package.xml +++ b/manymove_cpp_trees/package.xml @@ -27,6 +27,8 @@ simulation_interfaces vision_msgs rcpputils + ros2_medkit_fault_reporter + ros2_medkit_msgs ament_lint_auto ament_lint_common From 46cd956132addd3c09d388d44955a6210a8430b4 Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 22:10:27 +0200 Subject: [PATCH 04/15] feat(cpp_trees): add fault catalogue, FaultReporting and reporter install helper Three pieces for BT action node instrumentation: fault_codes.hpp (MANYMOVE_* constants), fault_reporting.hpp (the FaultReporting capability class), and installFaultReporter in main_imports_helper.hpp wired into all 15 BT entry-point mains. Capability class instead of direct FaultReporter member because BT.CPP factory constructs nodes with fixed signatures, so we pull the reporter from the blackboard in the FaultReporting ctor (same pattern as `node_` already fetched in every action node). --- .../manymove_cpp_trees/fault_codes.hpp | 99 ++++++++++++++++ .../manymove_cpp_trees/fault_reporting.hpp | 109 ++++++++++++++++++ .../main_imports_helper.hpp | 22 ++++ manymove_cpp_trees/src/bt_client.cpp | 1 + manymove_cpp_trees/src/bt_client_app_dual.cpp | 1 + manymove_cpp_trees/src/bt_client_cumotion.cpp | 1 + manymove_cpp_trees/src/bt_client_dual.cpp | 1 + .../src/bt_client_foundationpose.cpp | 1 + manymove_cpp_trees/src/bt_client_isaac.cpp | 1 + .../src/bt_client_mixed_pipelines.cpp | 1 + manymove_cpp_trees/src/bt_client_panda.cpp | 1 + .../src/bt_client_panda_cumotion.cpp | 1 + manymove_cpp_trees/src/bt_client_uf850.cpp | 1 + manymove_cpp_trees/src/bt_client_ur.cpp | 1 + manymove_cpp_trees/src/bt_client_xarm7.cpp | 1 + manymove_cpp_trees/src/collision_test.cpp | 1 + manymove_cpp_trees/src/tutorial_01.cpp | 1 + .../src/tutorial_01_complete.cpp | 1 + 18 files changed, 245 insertions(+) create mode 100644 manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp create mode 100644 manymove_cpp_trees/include/manymove_cpp_trees/fault_reporting.hpp 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..ac7f7c4 --- /dev/null +++ b/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp @@ -0,0 +1,99 @@ +// 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 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"; +// Required move_id input missing on the BT node. +inline constexpr char kPlannerMissingMoveId[] = "MANYMOVE_PLANNER_MISSING_MOVE_ID"; + +// -- 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"; +// CheckObjectExists action errored out (different from a clean negative). +inline constexpr char kObjectExistsCheckFailed[] = "MANYMOVE_OBJECT_EXISTS_CHECK_FAILED"; + +// -- 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"; + +// -- 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..aced5bc --- /dev/null +++ b/manymove_cpp_trees/include/manymove_cpp_trees/fault_reporting.hpp @@ -0,0 +1,109 @@ +// 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 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..e1534be 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 @@ -44,6 +44,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 +95,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/src/bt_client.cpp b/manymove_cpp_trees/src/bt_client.cpp index e0e1811..65fb5ec 100644 --- a/manymove_cpp_trees/src/bt_client.cpp +++ b/manymove_cpp_trees/src/bt_client.cpp @@ -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 5f0711c..95d6738 100644 --- a/manymove_cpp_trees/src/bt_client_app_dual.cpp +++ b/manymove_cpp_trees/src/bt_client_app_dual.cpp @@ -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 536c889..ed7895e 100644 --- a/manymove_cpp_trees/src/bt_client_cumotion.cpp +++ b/manymove_cpp_trees/src/bt_client_cumotion.cpp @@ -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 706a4f0..76819d4 100644 --- a/manymove_cpp_trees/src/bt_client_dual.cpp +++ b/manymove_cpp_trees/src/bt_client_dual.cpp @@ -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 119283a..3d131aa 100644 --- a/manymove_cpp_trees/src/bt_client_isaac.cpp +++ b/manymove_cpp_trees/src/bt_client_isaac.cpp @@ -42,6 +42,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_mixed_pipelines.cpp b/manymove_cpp_trees/src/bt_client_mixed_pipelines.cpp index 224e01c..4345988 100644 --- a/manymove_cpp_trees/src/bt_client_mixed_pipelines.cpp +++ b/manymove_cpp_trees/src/bt_client_mixed_pipelines.cpp @@ -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 4b1f49b..c1c9c7e 100644 --- a/manymove_cpp_trees/src/bt_client_panda.cpp +++ b/manymove_cpp_trees/src/bt_client_panda.cpp @@ -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 1d98ca1..1602cde 100644 --- a/manymove_cpp_trees/src/bt_client_panda_cumotion.cpp +++ b/manymove_cpp_trees/src/bt_client_panda_cumotion.cpp @@ -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 d683385..38fba05 100644 --- a/manymove_cpp_trees/src/bt_client_uf850.cpp +++ b/manymove_cpp_trees/src/bt_client_uf850.cpp @@ -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 494aadb..de3b27d 100644 --- a/manymove_cpp_trees/src/bt_client_ur.cpp +++ b/manymove_cpp_trees/src/bt_client_ur.cpp @@ -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 4029a72..8bf400e 100644 --- a/manymove_cpp_trees/src/bt_client_xarm7.cpp +++ b/manymove_cpp_trees/src/bt_client_xarm7.cpp @@ -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 5b7afc4..3dacc69 100644 --- a/manymove_cpp_trees/src/collision_test.cpp +++ b/manymove_cpp_trees/src/collision_test.cpp @@ -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/tutorial_01.cpp b/manymove_cpp_trees/src/tutorial_01.cpp index c75ae15..5ccf1ba 100644 --- a/manymove_cpp_trees/src/tutorial_01.cpp +++ b/manymove_cpp_trees/src/tutorial_01.cpp @@ -42,6 +42,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/tutorial_01_complete.cpp b/manymove_cpp_trees/src/tutorial_01_complete.cpp index 503d0c9..f7db2b0 100644 --- a/manymove_cpp_trees/src/tutorial_01_complete.cpp +++ b/manymove_cpp_trees/src/tutorial_01_complete.cpp @@ -41,6 +41,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 From aabaac57dda539995ea31f35d522273cd79fd307 Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 22:35:50 +0200 Subject: [PATCH 05/15] feat(cpp_trees): inherit FaultReporting in 21 BT action node classes Adds FaultReporting as a second base class to every Action node across planner/objects/signals/gripper/logic/isaac, plus the matching ctor init list call. Condition and decorator nodes are intentionally left out: their FAILURE return is normal control flow. --- .../manymove_cpp_trees/action_nodes_gripper.hpp | 8 +++++--- .../manymove_cpp_trees/action_nodes_isaac.hpp | 8 +++++--- .../manymove_cpp_trees/action_nodes_logic.hpp | 6 ++++-- .../manymove_cpp_trees/action_nodes_objects.hpp | 13 +++++++------ .../manymove_cpp_trees/action_nodes_planner.hpp | 5 +++-- .../manymove_cpp_trees/action_nodes_signals.hpp | 11 ++++++----- manymove_cpp_trees/src/action_nodes_gripper.cpp | 6 +++--- manymove_cpp_trees/src/action_nodes_isaac.cpp | 6 +++--- manymove_cpp_trees/src/action_nodes_logic.cpp | 4 ++-- manymove_cpp_trees/src/action_nodes_objects.cpp | 12 ++++++------ manymove_cpp_trees/src/action_nodes_planner.cpp | 3 ++- manymove_cpp_trees/src/action_nodes_signals.cpp | 10 +++++----- 12 files changed, 51 insertions(+), 41 deletions(-) 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/src/action_nodes_gripper.cpp b/manymove_cpp_trees/src/action_nodes_gripper.cpp index a36fe90..e2732c4 100644 --- a/manymove_cpp_trees/src/action_nodes_gripper.cpp +++ b/manymove_cpp_trees/src/action_nodes_gripper.cpp @@ -42,7 +42,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), @@ -201,7 +201,7 @@ 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) { @@ -318,7 +318,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..7a96434 100644 --- a/manymove_cpp_trees/src/action_nodes_isaac.cpp +++ b/manymove_cpp_trees/src/action_nodes_isaac.cpp @@ -60,7 +60,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 +162,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 +392,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"); diff --git a/manymove_cpp_trees/src/action_nodes_logic.cpp b/manymove_cpp_trees/src/action_nodes_logic.cpp index f93deed..1d24abe 100644 --- a/manymove_cpp_trees/src/action_nodes_logic.cpp +++ b/manymove_cpp_trees/src/action_nodes_logic.cpp @@ -230,7 +230,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) { @@ -365,7 +365,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( diff --git a/manymove_cpp_trees/src/action_nodes_objects.cpp b/manymove_cpp_trees/src/action_nodes_objects.cpp index 2f8544e..915c938 100644 --- a/manymove_cpp_trees/src/action_nodes_objects.cpp +++ b/manymove_cpp_trees/src/action_nodes_objects.cpp @@ -41,7 +41,7 @@ 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"; @@ -234,7 +234,7 @@ 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"; @@ -388,7 +388,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 @@ -569,7 +569,7 @@ 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 +723,7 @@ 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"; @@ -940,7 +940,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), diff --git a/manymove_cpp_trees/src/action_nodes_planner.cpp b/manymove_cpp_trees/src/action_nodes_planner.cpp index e3f86bf..2274e4b 100644 --- a/manymove_cpp_trees/src/action_nodes_planner.cpp +++ b/manymove_cpp_trees/src/action_nodes_planner.cpp @@ -41,6 +41,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), @@ -330,7 +331,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..c3f3412 100644 --- a/manymove_cpp_trees/src/action_nodes_signals.cpp +++ b/manymove_cpp_trees/src/action_nodes_signals.cpp @@ -43,7 +43,7 @@ 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) { @@ -189,7 +189,7 @@ 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."); @@ -329,7 +329,7 @@ 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) { @@ -464,7 +464,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), @@ -708,7 +708,7 @@ void ResetRobotStateAction::resultCallbackLoadTraj( 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), From 2a26a9c5cbc97a7a97934cded6f50f8c9315849c Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 22:38:37 +0200 Subject: [PATCH 06/15] feat(planner): emit medkit faults for collision, e-stop and retry exhaustion Four reportFault sites in MoveManipulatorAction: - kPlannerCollisionDetected on the onStart collision branch - kPlannerEstopTriggered when stop_execution is set before motion - kPlannerRetryAttempt on every failed attempt (soft fault, throttled locally by LocalFilter) - kPlannerRetriesExhausted once max_tries is reached reportFaultPassed pairs kPlannerRetryAttempt in the success branch so the local filter resets when a retry eventually succeeds. Skipped the input/blackboard-missing FAILURE returns (lines 96, 147, 174 and ResetTrajectories@356); those are programmer errors caught at startup, not operational faults. --- .../src/action_nodes_planner.cpp | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/manymove_cpp_trees/src/action_nodes_planner.cpp b/manymove_cpp_trees/src/action_nodes_planner.cpp index 2274e4b..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 @@ -106,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; } @@ -129,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; } @@ -220,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(), @@ -239,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( From 0ba48073ac02d6645b749bf6d5a9993b630f9d08 Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 22:43:25 +0200 Subject: [PATCH 07/15] feat(signals): emit medkit faults for IO failures, robot not ready, wait timeout Five reportFault sites covering SetOutput/GetInput action failures, CheckRobotState NOT_READY (CRITICAL), WaitForInput server-unavailable and the wait-timeout path. reportFaultPassed pairs kRobotNotReady on CheckRobotState success and kSignalWaitInputTimeout on the WaitForInput desired-value branch so LocalFilter resets cleanly. --- .../src/action_nodes_signals.cpp | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/manymove_cpp_trees/src/action_nodes_signals.cpp b/manymove_cpp_trees/src/action_nodes_signals.cpp index c3f3412..11c2cd1 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 @@ -143,6 +144,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; } } @@ -285,6 +290,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; } } @@ -401,6 +410,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 +419,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; } } @@ -762,6 +779,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 +824,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 +841,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; } } From 718196a56811b2dd06d0eca42fe67a60bd5deabc Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 22:45:38 +0200 Subject: [PATCH 08/15] feat(objects): emit medkit faults on add/remove/attach/get-pose/wait-timeout Five reportFault sites cover the operational failures of AddCollisionObjectAction, RemoveCollisionObjectAction, AttachDetachObjectAction, GetObjectPoseAction and WaitForObjectAction. WaitForObject pairs reportFaultPassed in the success branch. CheckObjectExistsAction is intentionally not instrumented: its FAILURE on \"object missing\" is normal control flow used by callers as a condition, not an operational fault. --- .../src/action_nodes_objects.cpp | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/manymove_cpp_trees/src/action_nodes_objects.cpp b/manymove_cpp_trees/src/action_nodes_objects.cpp index 915c938..f36d4ce 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 @@ -161,6 +162,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; } } @@ -313,6 +317,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; } } @@ -495,6 +502,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; } } @@ -872,6 +883,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; } } @@ -1037,6 +1051,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 +1068,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; } } From 9fe2005dbc3317b840052a3c7b33e69f6665678c Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 22:47:23 +0200 Subject: [PATCH 09/15] feat(gripper): emit medkit faults on command/traj operational failures Four reportFault sites: GripperCommand server timeout and goal-not-reached, GripperTraj server unavailable and trajectory-failed result. PublishJointStateAction throws on bad inputs and never returns FAILURE, so it has nothing to instrument. --- .../src/action_nodes_gripper.cpp | 26 ++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/manymove_cpp_trees/src/action_nodes_gripper.cpp b/manymove_cpp_trees/src/action_nodes_gripper.cpp index e2732c4..e8d2fad 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; @@ -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() @@ -226,6 +236,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; } @@ -277,8 +290,13 @@ BT::NodeStatus GripperTrajAction::onRunning() 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() From 1f599c242210f9149f9d0af4308f41465fc9c9ad Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 22:48:39 +0200 Subject: [PATCH 10/15] feat(logic): emit medkit faults for TF lookup and WaitForKeyBool timeout GetLinkPoseAction reports kTfLookupFailed when tf2 raises a TransformException. WaitForKeyBool reports kWaitKeyTimeout on the timeout branch and pairs reportFaultPassed in the success path. Adds kWaitKeyTimeout to fault_codes.hpp. Configuration-error FAILUREs (missing inputs, wrong vector sizes) are left out, as are condition-style nodes (CheckPoseDistance, CheckPoseBounds, CheckKeyBoolValue) where FAILURE is expected control flow. --- .../include/manymove_cpp_trees/fault_codes.hpp | 2 ++ manymove_cpp_trees/src/action_nodes_logic.cpp | 12 ++++++++++++ 2 files changed, 14 insertions(+) diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp index ac7f7c4..e52f49f 100644 --- a/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp +++ b/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp @@ -83,6 +83,8 @@ inline constexpr char kRobotResetFailed[] = "MANYMOVE_ROBOT_RESET_FAIL // -- 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. diff --git a/manymove_cpp_trees/src/action_nodes_logic.cpp b/manymove_cpp_trees/src/action_nodes_logic.cpp index 1d24abe..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 @@ -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; } } @@ -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; } From 815ef95711249be2aea4a5ed80d525cf4822b0cd Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 22:49:54 +0200 Subject: [PATCH 11/15] feat(isaac): emit medkit faults on FoundationPose detection timeouts Two reportFault sites in FoundationPoseAlignmentNode: detection topic silent for too long, and no detection passing the target/min_score filter within the timeout. Both raise kIsaacFoundationPoseFailed. GetEntityPoseNode/SetEntityPoseNode service-error paths and Isaac TF timeouts are left for the isaac demo work in Phase 3, where they'll have specific fault codes once the demo wiring exposes them. --- manymove_cpp_trees/src/action_nodes_isaac.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/manymove_cpp_trees/src/action_nodes_isaac.cpp b/manymove_cpp_trees/src/action_nodes_isaac.cpp index 7a96434..18dc55d 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 @@ -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; } } From 5d640c7535a4ad0ce6eb89f9889b4032f3ed3a9e Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 22:55:09 +0200 Subject: [PATCH 12/15] test(cpp_trees): add fault reporting roundtrip test with FakeFaultManager fake_fault_manager.hpp is a small fixture that hosts the /fault_manager/report_fault service and records every accepted request, matching the existing FakeXxxServer pattern in this test directory. test_fault_reporting.cpp uses it to verify the end-to-end plumbing: - installFaultReporter puts a reporter on the blackboard - FaultReporting::reportFault forwards FAILED events with the right fault_code, severity, description and source_id (FQN of the BT client node) - reportFaultPassed forwards a PASSED event for healing - mixin silently no-ops when no reporter is installed (e.g. unit tests building a tree without bt_client scaffolding) Per-instrumentation-site assertions are out of scope here; those would need to drive every BT action node through real action server fakes and belong with the demo work in Phase 3. --- manymove_cpp_trees/CMakeLists.txt | 3 + .../test/fake_fault_manager.hpp | 119 ++++++++++++ .../test/test_fault_reporting.cpp | 172 ++++++++++++++++++ 3 files changed, 294 insertions(+) create mode 100644 manymove_cpp_trees/test/fake_fault_manager.hpp create mode 100644 manymove_cpp_trees/test/test_fault_reporting.cpp diff --git a/manymove_cpp_trees/CMakeLists.txt b/manymove_cpp_trees/CMakeLists.txt index f8c28b6..98db41e 100644 --- a/manymove_cpp_trees/CMakeLists.txt +++ b/manymove_cpp_trees/CMakeLists.txt @@ -183,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/test/fake_fault_manager.hpp b/manymove_cpp_trees/test/fake_fault_manager.hpp new file mode 100644 index 0000000..9f00d97 --- /dev/null +++ b/manymove_cpp_trees/test/fake_fault_manager.hpp @@ -0,0 +1,119 @@ +// 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 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__TEST__FAKE_FAULT_MANAGER_HPP_ +#define MANYMOVE_CPP_TREES__TEST__FAKE_FAULT_MANAGER_HPP_ + +#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 // MANYMOVE_CPP_TREES__TEST__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..ca6da04 --- /dev/null +++ b/manymove_cpp_trees/test/test_fault_reporting.cpp @@ -0,0 +1,172 @@ +// 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 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 "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; +} From 7c6453bc02c34d4ebd9400ce0419cac6f25854a0 Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 23:02:23 +0200 Subject: [PATCH 13/15] docs(cpp_trees): add FAULT_CODES catalogue + README/CHANGELOG entries - docs/FAULT_CODES.md: full table of MANYMOVE_* codes with severity, trigger condition and pairing semantics, grouped by subsystem. - README.md: short note pointing fork users at the medkit dependency and the catalogue. - CHANGELOG.rst: entry under Forthcoming summarising the BT node rename, the hard medkit dep and the dropped topic_based_ros2_control declaration. - ament_uncrustify --reformat run across the modified manymove_cpp_trees sources. - Lint fixes for new files (cpplint header order / memory include / guard style; ament_copyright BSD-3 boilerplate match). The remaining test_action_nodes_gripper failure is a pre-existing convertFromString registration issue in upstream manymove (verified by running the test from the parent commit before any medkit changes). --- CHANGELOG.rst | 13 ++- README.md | 11 +++ docs/FAULT_CODES.md | 96 +++++++++++++++++++ .../manymove_cpp_trees/fault_codes.hpp | 44 ++++----- .../manymove_cpp_trees/fault_reporting.hpp | 11 ++- .../main_imports_helper.hpp | 1 + .../src/action_nodes_gripper.cpp | 3 +- .../src/action_nodes_objects.cpp | 12 ++- .../src/action_nodes_signals.cpp | 9 +- .../test/fake_fault_manager.hpp | 10 +- .../test/test_fault_reporting.cpp | 8 +- 11 files changed, 175 insertions(+), 43 deletions(-) create mode 100644 docs/FAULT_CODES.md 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..2e38013 --- /dev/null +++ b/docs/FAULT_CODES.md @@ -0,0 +1,96 @@ +# 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. | + +`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. | +| `MANYMOVE_ROBOT_NOT_READY` | CRITICAL | `CheckRobotStateAction` reports `ready=false`. | `reportFaultPassed` once the robot reports `ready=true`. | + +## 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, or no detection passed the `target_id`/`min_score` filter within the configured timeout. | - | + +`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_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp index e52f49f..86e7f91 100644 --- a/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp +++ b/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp @@ -10,7 +10,7 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// * Neither the name of Selfpatch.ai nor the names of its contributors +// * 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. // @@ -44,57 +44,57 @@ 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"; +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"; +inline constexpr char kPlannerRetryAttempt[] = "MANYMOVE_PLANNER_RETRY_ATTEMPT"; // All retries exhausted, motion aborted. -inline constexpr char kPlannerRetriesExhausted[] = "MANYMOVE_PLANNER_RETRIES_EXHAUSTED"; +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"; +inline constexpr char kPlannerEstopTriggered[] = "MANYMOVE_PLANNER_ESTOP_TRIGGERED"; // Required move_id input missing on the BT node. -inline constexpr char kPlannerMissingMoveId[] = "MANYMOVE_PLANNER_MISSING_MOVE_ID"; +inline constexpr char kPlannerMissingMoveId[] = "MANYMOVE_PLANNER_MISSING_MOVE_ID"; // -- Object manager --------------------------------------------------------- // AddCollisionObject action returned failure (planning scene reject). -inline constexpr char kObjectAddFailed[] = "MANYMOVE_OBJECT_ADD_FAILED"; +inline constexpr char kObjectAddFailed[] = "MANYMOVE_OBJECT_ADD_FAILED"; // RemoveCollisionObject action returned failure. -inline constexpr char kObjectRemoveFailed[] = "MANYMOVE_OBJECT_REMOVE_FAILED"; +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"; +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"; +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"; +inline constexpr char kObjectWaitTimeout[] = "MANYMOVE_OBJECT_WAIT_TIMEOUT"; // CheckObjectExists action errored out (different from a clean negative). -inline constexpr char kObjectExistsCheckFailed[] = "MANYMOVE_OBJECT_EXISTS_CHECK_FAILED"; +inline constexpr char kObjectExistsCheckFailed[] = "MANYMOVE_OBJECT_EXISTS_CHECK_FAILED"; // -- Signals (gripper, IO, robot state) ------------------------------------- // SetOutputAction returned a failed result from the action server. -inline constexpr char kSignalSetOutputFailed[] = "MANYMOVE_SIGNAL_SET_OUTPUT_FAILED"; +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"; +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"; +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"; +inline constexpr char kRobotNotReady[] = "MANYMOVE_ROBOT_NOT_READY"; // ResetRobotStateAction failed (controller unload / state reset / load step). -inline constexpr char kRobotResetFailed[] = "MANYMOVE_ROBOT_RESET_FAILED"; +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"; +inline constexpr char kTfLookupFailed[] = "MANYMOVE_TF_LOOKUP_FAILED"; // WaitForKeyBool elapsed without observing the expected blackboard value. -inline constexpr char kWaitKeyTimeout[] = "MANYMOVE_WAIT_KEY_TIMEOUT"; +inline constexpr char kWaitKeyTimeout[] = "MANYMOVE_WAIT_KEY_TIMEOUT"; // -- Gripper ---------------------------------------------------------------- // GripperCommandAction goal aborted, canceled, or rejected. -inline constexpr char kGripperCommandFailed[] = "MANYMOVE_GRIPPER_COMMAND_FAILED"; +inline constexpr char kGripperCommandFailed[] = "MANYMOVE_GRIPPER_COMMAND_FAILED"; // GripperTrajAction trajectory execution failed. -inline constexpr char kGripperTrajFailed[] = "MANYMOVE_GRIPPER_TRAJ_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"; +inline constexpr char kIsaacFoundationPoseFailed[] = "MANYMOVE_ISAAC_FOUNDATION_POSE_FAILED"; } // namespace manymove_cpp_trees::fault_codes diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/fault_reporting.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/fault_reporting.hpp index aced5bc..807fe33 100644 --- a/manymove_cpp_trees/include/manymove_cpp_trees/fault_reporting.hpp +++ b/manymove_cpp_trees/include/manymove_cpp_trees/fault_reporting.hpp @@ -10,7 +10,7 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// * Neither the name of Selfpatch.ai nor the names of its contributors +// * 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. // @@ -29,10 +29,11 @@ #ifndef MANYMOVE_CPP_TREES__FAULT_REPORTING_HPP_ #define MANYMOVE_CPP_TREES__FAULT_REPORTING_HPP_ +#include + #include #include -#include #include #include @@ -47,9 +48,9 @@ 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 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 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 e1534be..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 diff --git a/manymove_cpp_trees/src/action_nodes_gripper.cpp b/manymove_cpp_trees/src/action_nodes_gripper.cpp index e8d2fad..aaba518 100644 --- a/manymove_cpp_trees/src/action_nodes_gripper.cpp +++ b/manymove_cpp_trees/src/action_nodes_gripper.cpp @@ -211,7 +211,8 @@ void GripperCommandAction::feedbackCallback( // ------------------------------------------------- GripperTrajAction::GripperTrajAction(const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), 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) { diff --git a/manymove_cpp_trees/src/action_nodes_objects.cpp b/manymove_cpp_trees/src/action_nodes_objects.cpp index f36d4ce..9d4e4de 100644 --- a/manymove_cpp_trees/src/action_nodes_objects.cpp +++ b/manymove_cpp_trees/src/action_nodes_objects.cpp @@ -42,7 +42,8 @@ namespace manymove_cpp_trees AddCollisionObjectAction::AddCollisionObjectAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), 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"; @@ -238,7 +239,8 @@ void AddCollisionObjectAction::resultCallback( RemoveCollisionObjectAction::RemoveCollisionObjectAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), 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"; @@ -580,7 +582,8 @@ void AttachDetachObjectAction::resultCallback( CheckObjectExistsAction::CheckObjectExistsAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), 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"; @@ -734,7 +737,8 @@ void CheckObjectExistsAction::resultCallback( GetObjectPoseAction::GetObjectPoseAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), 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"; diff --git a/manymove_cpp_trees/src/action_nodes_signals.cpp b/manymove_cpp_trees/src/action_nodes_signals.cpp index 11c2cd1..88bad9e 100644 --- a/manymove_cpp_trees/src/action_nodes_signals.cpp +++ b/manymove_cpp_trees/src/action_nodes_signals.cpp @@ -44,7 +44,8 @@ namespace manymove_cpp_trees // ------------------------------------------------------------------ SetOutputAction::SetOutputAction(const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), 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) { @@ -194,7 +195,8 @@ void SetOutputAction::resultCallback(const GoalHandleSetOutput::WrappedResult & // ------------------------------------------------------------------ GetInputAction::GetInputAction(const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), 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."); @@ -338,7 +340,8 @@ void GetInputAction::resultCallback(const GoalHandleGetInput::WrappedResult & wr CheckRobotStateAction::CheckRobotStateAction( const std::string & name, const BT::NodeConfiguration & config) -: BT::StatefulActionNode(name, config), FaultReporting(config.blackboard), 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) { diff --git a/manymove_cpp_trees/test/fake_fault_manager.hpp b/manymove_cpp_trees/test/fake_fault_manager.hpp index 9f00d97..fc5cbe2 100644 --- a/manymove_cpp_trees/test/fake_fault_manager.hpp +++ b/manymove_cpp_trees/test/fake_fault_manager.hpp @@ -10,7 +10,7 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// * Neither the name of Selfpatch.ai nor the names of its contributors +// * 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. // @@ -26,12 +26,14 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#ifndef MANYMOVE_CPP_TREES__TEST__FAKE_FAULT_MANAGER_HPP_ -#define MANYMOVE_CPP_TREES__TEST__FAKE_FAULT_MANAGER_HPP_ +#ifndef FAKE_FAULT_MANAGER_HPP_ +#define FAKE_FAULT_MANAGER_HPP_ #include +#include #include #include +#include #include #include @@ -116,4 +118,4 @@ class FakeFaultManager } // namespace manymove_cpp_trees::test -#endif // MANYMOVE_CPP_TREES__TEST__FAKE_FAULT_MANAGER_HPP_ +#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 index ca6da04..14ed257 100644 --- a/manymove_cpp_trees/test/test_fault_reporting.cpp +++ b/manymove_cpp_trees/test/test_fault_reporting.cpp @@ -10,7 +10,7 @@ // notice, this list of conditions and the following disclaimer in the // documentation and/or other materials provided with the distribution. // -// * Neither the name of Selfpatch.ai nor the names of its contributors +// * 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. // @@ -27,11 +27,12 @@ // POSSIBILITY OF SUCH DAMAGE. #include +#include #include #include +#include -#include #include #include @@ -113,7 +114,8 @@ TEST_F(FaultReportingFixture, MixinForwardsErrorReportToFakeManager) 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().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())); From ebb776d5e62ab50b96c182812ae1e3493a7e8191 Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Sat, 9 May 2026 23:16:36 +0200 Subject: [PATCH 14/15] ci: install ros2_medkit deb packages so the medkit overlay build links After wiring ros2_medkit_fault_reporter into manymove_cpp_trees the CI job needs the medkit packages on the runner. Pulling them via apt from rosdistro keeps the workflow simple (no source overlay clone). --- .github/workflows/ci.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 34977c5..d833372 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -63,6 +63,10 @@ jobs: ccache \ ros-$ROS_DISTRO-topic-based-ros2-control \ python3-colcon-common-extensions python3-rosdep git \ + ros-$ROS_DISTRO-ros2-medkit-fault-reporter \ + ros-$ROS_DISTRO-ros2-medkit-msgs \ + ros-$ROS_DISTRO-ros2-medkit-fault-manager \ + ros-$ROS_DISTRO-ros2-medkit-cmake \ ros-$ROS_DISTRO-moveit \ ros-$ROS_DISTRO-ur \ ros-$ROS_DISTRO-ur-moveit-config \ From cf5387177ffef55c92565096a415a6e22c13e490 Mon Sep 17 00:00:00 2001 From: Michal Faferek Date: Tue, 12 May 2026 21:55:01 +0200 Subject: [PATCH 15/15] fix: address self-review findings - Wire kRobotResetFailed across ResetRobotStateAction failure branches (goal rejection + non-SUCCEEDED result for reset, unload-traj and load-traj steps). - Instrument Isaac TF transform timeouts with kIsaacFoundationPoseFailed to match the sibling no-message and no-valid-detection timeouts. - Replace silent FAILURE in GripperTrajAction::onRunning !goal_sent_ with BT::RuntimeError, matching the programmer-error policy. - Add reportFaultPassed on onHalted for WaitForInputAction and WaitForObjectAction so abandoned waits do not leave kSignalWaitInputTimeout / kObjectWaitTimeout lingering in FaultManager. - Drop orphan codes kPlannerMissingMoveId (programmer-error path uses throw) and kObjectExistsCheckFailed (CheckObjectExists is intentionally not instrumented; FAILURE is normal control flow there). - Pin medkit apt packages to MEDKIT_VERSION=0.4.0-1 so an upstream bump does not silently change CI behaviour. - Drop humble from the CI matrix until ros-humble-ros2-medkit-* debs appear on packages.ros.org (rosdistro entry exists; debs do not yet). - Update docs/FAULT_CODES.md: add kRobotResetFailed, expand the Isaac FoundationPose entry to cover TF timeouts, and document the reportFaultPassed-on-halt pairing for both wait codes. --- .github/workflows/ci.yml | 18 +++++--- docs/FAULT_CODES.md | 7 ++-- .../manymove_cpp_trees/fault_codes.hpp | 4 -- .../src/action_nodes_gripper.cpp | 5 ++- manymove_cpp_trees/src/action_nodes_isaac.cpp | 8 ++++ .../src/action_nodes_objects.cpp | 5 +++ .../src/action_nodes_signals.cpp | 41 +++++++++++++++++++ 7 files changed, 75 insertions(+), 13 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d833372..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,10 +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 \ - ros-$ROS_DISTRO-ros2-medkit-msgs \ - ros-$ROS_DISTRO-ros2-medkit-fault-manager \ - ros-$ROS_DISTRO-ros2-medkit-cmake \ + 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/docs/FAULT_CODES.md b/docs/FAULT_CODES.md index 2e38013..727a855 100644 --- a/docs/FAULT_CODES.md +++ b/docs/FAULT_CODES.md @@ -35,7 +35,7 @@ node sources; the source-of-truth string constants live in | `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. | +| `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. @@ -46,8 +46,9 @@ control flow used by callers as a condition; it is not instrumented. |------|----------|---------|------| | `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. | +| `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`) @@ -67,7 +68,7 @@ control flow used by callers as a condition; it is not instrumented. | Code | Severity | Trigger | Pair | |------|----------|---------|------| -| `MANYMOVE_ISAAC_FOUNDATION_POSE_FAILED` | ERROR | `FoundationPoseAlignmentNode` timed out waiting for detections, or no detection passed the `target_id`/`min_score` filter within the configured timeout. | - | +| `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 diff --git a/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp b/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp index 86e7f91..04fdea1 100644 --- a/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp +++ b/manymove_cpp_trees/include/manymove_cpp_trees/fault_codes.hpp @@ -51,8 +51,6 @@ inline constexpr char kPlannerRetryAttempt[] = "MANYMOVE_PLANNER_RETRY_ATTEMPT"; 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"; -// Required move_id input missing on the BT node. -inline constexpr char kPlannerMissingMoveId[] = "MANYMOVE_PLANNER_MISSING_MOVE_ID"; // -- Object manager --------------------------------------------------------- // AddCollisionObject action returned failure (planning scene reject). @@ -65,8 +63,6 @@ inline constexpr char kObjectAttachFailed[] = "MANYMOVE_OBJECT_ATTACH_FAILED"; 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"; -// CheckObjectExists action errored out (different from a clean negative). -inline constexpr char kObjectExistsCheckFailed[] = "MANYMOVE_OBJECT_EXISTS_CHECK_FAILED"; // -- Signals (gripper, IO, robot state) ------------------------------------- // SetOutputAction returned a failed result from the action server. diff --git a/manymove_cpp_trees/src/action_nodes_gripper.cpp b/manymove_cpp_trees/src/action_nodes_gripper.cpp index aaba518..a5f38fe 100644 --- a/manymove_cpp_trees/src/action_nodes_gripper.cpp +++ b/manymove_cpp_trees/src/action_nodes_gripper.cpp @@ -284,7 +284,10 @@ 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_) { diff --git a/manymove_cpp_trees/src/action_nodes_isaac.cpp b/manymove_cpp_trees/src/action_nodes_isaac.cpp index 18dc55d..88c6a36 100644 --- a/manymove_cpp_trees/src/action_nodes_isaac.cpp +++ b/manymove_cpp_trees/src/action_nodes_isaac.cpp @@ -608,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; } } @@ -646,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_objects.cpp b/manymove_cpp_trees/src/action_nodes_objects.cpp index 9d4e4de..03c3e09 100644 --- a/manymove_cpp_trees/src/action_nodes_objects.cpp +++ b/manymove_cpp_trees/src/action_nodes_objects.cpp @@ -1119,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_signals.cpp b/manymove_cpp_trees/src/action_nodes_signals.cpp index 88bad9e..b76cd8d 100644 --- a/manymove_cpp_trees/src/action_nodes_signals.cpp +++ b/manymove_cpp_trees/src/action_nodes_signals.cpp @@ -657,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.", @@ -672,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; } @@ -684,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(), @@ -696,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"); } } @@ -709,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(), @@ -721,8 +747,18 @@ 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"); } } @@ -877,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; }