fix(cpp_trees): address PR #1 review

- ResetRobotStateAction: latch fault_reported_ to coalesce up to 6
  kRobotResetFailed callback paths into a single FAILED event.
- MoveManipulatorAction: emit reportFaultPassed for kPlannerCollision
  Detected and kPlannerEstopTriggered on the early path of onStart when
  the corresponding blackboard flag reads false, so the fault clears
  after recovery. Guard the per-attempt PASSED on success with
  current_try_>0; emit kPlannerRetryAttempt PASSED in onHalted.
- WaitForInputAction: track timeout_reported_ so reportFaultPassed
  fires only when a FAILED preceded it (onRunning success and
  onHalted), instead of accumulating spurious PASSED events.
- PublishJointStateAction, ResetTrajectories: document the empty
  FaultReporting inheritance as reserved for future failure modes.
- installFaultReporter: accept optional service_name override for
  namespaced test rigs and multi-robot setups.

Author: mfaferek93

manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_gripper.hpp

@@ -136,6 +136,11 @@ class GripperTrajAction : public BT::StatefulActionNode, public FaultReporting
 // PublishJointStateAction
 // =======================================================
 
+// NOTE: this node currently has no fault sites — the call is fire-and-forget
+// via the publisher API and cannot fail in a way worth reporting. The
+// FaultReporting base is kept so that any future failure mode (e.g. detection
+// of a downstream subscriber drop) can emit through the same channel as the
+// rest of the BT without changing the inheritance.
 class PublishJointStateAction : public BT::SyncActionNode, public FaultReporting
 {
 public:

manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_planner.hpp

@@ -113,6 +113,11 @@ class MoveManipulatorAction : public BT::StatefulActionNode, public FaultReporti
  *
  * 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.
+ *
+ * NOTE: this node has no fault sites — blackboard writes do not fail in a way
+ * worth reporting. The FaultReporting base is kept so that any future failure
+ * mode (e.g. missing move_id list, malformed input) can emit through the same
+ * channel as the rest of the BT without changing the inheritance.
  */
 class ResetTrajectories : public BT::SyncActionNode, public FaultReporting
 {

manymove_cpp_trees/include/manymove_cpp_trees/action_nodes_signals.hpp

@@ -248,6 +248,14 @@ class ResetRobotStateAction : public BT::StatefulActionNode, public FaultReporti
   bool reset_goal_sent_;
   bool load_goal_sent_;
 
+  // One failed reset can fan out across rejection + result-failure callbacks
+  // for unload-trajectory, reset-robot-state and load-trajectory (up to 6
+  // call sites). Without coalescing, those would all emit
+  // kRobotResetFailed at kSeverityError (>= bypass_severity), driving the
+  // AUTOSAR-DEM debounce counter past confirmation_threshold on a single
+  // logical incident. Latch on first emission per run; reset in onStart.
+  bool fault_reported_;
+
   std::string computed_controller_name_;
 
   ResetRobotState::Result action_result_;
@@ -313,6 +321,12 @@ class WaitForInputAction : public BT::StatefulActionNode, public FaultReporting
   // last read from the server
   bool last_success_;
   int last_value_;
+
+  // Track whether kSignalWaitInputTimeout was actually emitted this run so
+  // onHalted only fires reportFaultPassed when a FAILED preceded it.
+  // Without this, every halt accumulated a spurious PASSED, which biases
+  // LocalFilter::should_forward_passed counters in the medkit reporter.
+  bool timeout_reported_;
 };
 
 }  // namespace manymove_cpp_trees

manymove_cpp_trees/include/manymove_cpp_trees/main_imports_helper.hpp

@@ -106,10 +106,15 @@ namespace manymove_cpp_trees
 //
 // 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)
+//
+// service_name lets callers remap the FaultManager service for namespaced
+// test rigs or multi-robot setups (e.g. "/robot1/fault_manager/report_fault").
+inline void installFaultReporter(
+  BT::Blackboard::Ptr blackboard, rclcpp::Node::SharedPtr node,
+  const std::string & service_name = "/fault_manager/report_fault")
 {
   auto reporter = std::make_shared<ros2_medkit_fault_reporter::FaultReporter>(
-    node, node->get_fully_qualified_name());
+    node, node->get_fully_qualified_name(), service_name);
   blackboard->set(manymove_cpp_trees::kFaultReporterBlackboardKey, reporter);
 }
 

manymove_cpp_trees/src/action_nodes_planner.cpp

@@ -112,6 +112,10 @@ BT::NodeStatus MoveManipulatorAction::onStart()
       "collision detected on " + robot_prefix_ + " before motion start");
     return BT::NodeStatus::FAILURE;
   }
+  // Heal any prior CONFIRMED collision once the flag has been cleared.
+  // Without this, the fault lingers in FaultManager after the operator /
+  // recovery cleared the underlying condition on hardware.
+  reportFaultPassed(fault_codes::kPlannerCollisionDetected);
 
   // Read move_id.
   if (!getInput<std::string>("move_id", move_id_)) {
@@ -138,6 +142,9 @@ BT::NodeStatus MoveManipulatorAction::onStart()
       "stop_execution flag set on " + robot_prefix_ + " before motion start");
     return BT::NodeStatus::FAILURE;
   }
+  // Heal any prior CONFIRMED e-stop once the flag has been cleared by the
+  // operator. Without this, the CRITICAL fault lingers after release.
+  reportFaultPassed(fault_codes::kPlannerEstopTriggered);
 
   return BT::NodeStatus::RUNNING;
 }
@@ -227,8 +234,13 @@ 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);
+      // Heal the per-attempt soft fault only when a retry actually occurred.
+      // On first-attempt success current_try_ is still 0 — no FAILED was
+      // ever emitted, and a stray PASSED biases
+      // LocalFilter::should_forward_passed in the medkit reporter.
+      if (current_try_ > 0) {
+        reportFaultPassed(fault_codes::kPlannerRetryAttempt);
+      }
       return BT::NodeStatus::SUCCESS;
     } else {
       config().blackboard->set("trajectory_" + move_id_, trajectory_msgs::msg::JointTrajectory());
@@ -291,6 +303,14 @@ void MoveManipulatorAction::onHalted()
 
   // HMI message
   setHMIMessage(config().blackboard, robot_prefix_, "MOTION HALTED", "red");
+
+  // Heal the per-attempt soft fault on halt, mirroring WaitForInputAction::
+  // onHalted and WaitForObjectAction::onHalted. Without this, a halted
+  // retry loop leaves a lingering kPlannerRetryAttempt soft fault in
+  // FaultManager until the next successful attempt.
+  if (current_try_ > 0) {
+    reportFaultPassed(fault_codes::kPlannerRetryAttempt);
+  }
 }
 
 void MoveManipulatorAction::goalResponseCallback(

manymove_cpp_trees/src/action_nodes_signals.cpp

@@ -488,7 +488,11 @@ ResetRobotStateAction::ResetRobotStateAction(
   goal_sent_(false),
   result_received_(false),
   unload_traj_success_(false),
-  load_traj_success_(false)
+  load_traj_success_(false),
+  unload_goal_sent_(false),
+  reset_goal_sent_(false),
+  load_goal_sent_(false),
+  fault_reported_(false)
 {
   if (!config.blackboard) {
     throw BT::RuntimeError("ResetRobotStateAction: no blackboard provided.");
@@ -553,6 +557,7 @@ BT::NodeStatus ResetRobotStateAction::onStart()
   unload_goal_sent_ = false;
   reset_goal_sent_ = false;
   load_goal_sent_ = false;
+  fault_reported_ = false;
 
   // Step 1: Call UnloadTrajController action
   UnloadTrajController::Goal unload_traj_goal;
@@ -657,10 +662,13 @@ 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");
+    if (!fault_reported_) {
+      fault_reported_ = 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.",
@@ -676,10 +684,13 @@ 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");
+    if (!fault_reported_) {
+      fault_reported_ = true;
+      reportFault(
+        fault_codes::kRobotResetFailed, kSeverityError,
+        std::string("ResetRobotStateAction [") + name() +
+        "]: reset_robot_state action aborted or failed");
+    }
   }
   result_received_ = true;
 }
@@ -692,10 +703,13 @@ 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");
+    if (!fault_reported_) {
+      fault_reported_ = true;
+      reportFault(
+        fault_codes::kRobotResetFailed, kSeverityError,
+        std::string("ResetRobotStateAction [") + name() +
+        "]: unload_trajectory_controller goal rejected by server");
+    }
   } else {
     RCLCPP_INFO(
       node_->get_logger(),
@@ -708,18 +722,22 @@ void ResetRobotStateAction::resultCallbackUnloadTraj(
 {
   if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
     unload_traj_success_ = wrapped_result.result->success;
-    if (!unload_traj_success_) {
+    if (!unload_traj_success_ && !fault_reported_) {
+      fault_reported_ = true;
       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");
+    if (!fault_reported_) {
+      fault_reported_ = true;
+      reportFault(
+        fault_codes::kRobotResetFailed, kSeverityError,
+        std::string("ResetRobotStateAction [") + name() +
+        "]: unload_trajectory_controller action aborted or failed");
+    }
   }
 }
 
@@ -731,10 +749,13 @@ 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");
+    if (!fault_reported_) {
+      fault_reported_ = true;
+      reportFault(
+        fault_codes::kRobotResetFailed, kSeverityError,
+        std::string("ResetRobotStateAction [") + name() +
+        "]: load_trajectory_controller goal rejected by server");
+    }
   } else {
     RCLCPP_INFO(
       node_->get_logger(),
@@ -747,18 +768,22 @@ void ResetRobotStateAction::resultCallbackLoadTraj(
 {
   if (wrapped_result.code == rclcpp_action::ResultCode::SUCCEEDED) {
     load_traj_success_ = wrapped_result.result->success;
-    if (!load_traj_success_) {
+    if (!load_traj_success_ && !fault_reported_) {
+      fault_reported_ = true;
       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");
+    if (!fault_reported_) {
+      fault_reported_ = true;
+      reportFault(
+        fault_codes::kRobotResetFailed, kSeverityError,
+        std::string("ResetRobotStateAction [") + name() +
+        "]: load_trajectory_controller action aborted or failed");
+    }
   }
 }
 
@@ -768,7 +793,8 @@ WaitForInputAction::WaitForInputAction(
   goal_sent_(false),
   result_received_(false),
   last_success_(false),
-  last_value_(0)
+  last_value_(0),
+  timeout_reported_(false)
 {
   // Obtain the ROS2 node from the blackboard
   if (!config.blackboard) {
@@ -786,6 +812,7 @@ BT::NodeStatus WaitForInputAction::onStart()
   result_received_ = false;
   last_success_ = false;
   last_value_ = 0;
+  timeout_reported_ = false;
 
   // Read ports
   if (!getInput<std::string>("io_type", io_type_)) {
@@ -863,8 +890,13 @@ BT::NodeStatus WaitForInputAction::onRunning()
         std::to_string(desired_value_),
         "green");
 
-      // Pair: clear timeout filter once the desired value is observed.
-      reportFaultPassed(fault_codes::kSignalWaitInputTimeout);
+      // Pair: only clear if we actually emitted the timeout earlier. A
+      // first-pass success has no FAILED to heal and a stray PASSED biases
+      // LocalFilter::should_forward_passed.
+      if (timeout_reported_) {
+        reportFaultPassed(fault_codes::kSignalWaitInputTimeout);
+        timeout_reported_ = false;
+      }
       return BT::NodeStatus::SUCCESS;
     }
 
@@ -884,6 +916,7 @@ BT::NodeStatus WaitForInputAction::onRunning()
           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_));
+        timeout_reported_ = true;
         return BT::NodeStatus::FAILURE;
       }
     }
@@ -914,10 +947,14 @@ void WaitForInputAction::onHalted()
     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);
+  // mid-wait, the timeout condition no longer holds. Only emit if we
+  // actually reported the FAILED earlier — otherwise the PASSED would
+  // accumulate in LocalFilter::should_forward_passed and bias subsequent
+  // healing decisions.
+  if (timeout_reported_) {
+    reportFaultPassed(fault_codes::kSignalWaitInputTimeout);
+    timeout_reported_ = false;
+  }
   goal_sent_ = false;
   result_received_ = false;
 }