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.

Author: mfaferek93

.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 \

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

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.

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_) {

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;
         }
       }

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;
 }

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;
 }