Fix recovery cmd_vel hold behavior

contributions/recovery-safety/xbattlax/README.md

@@ -78,6 +78,8 @@ The tests cover:
 - e-stop and safety events entering safe pause
 - ignored duplicate triggers while already recovering
 - JSON status shape
+- held `/cmd_vel` publishing so motion recoveries outlive base watchdog timeouts
+- a separate completion timeout for delegated commands such as `clear_costmap`
 
 ## Run
 
@@ -112,6 +114,7 @@ ros2 topic pub --once /oomwoo/safety/e_stop std_msgs/msg/Bool "{data: true}"
   future adapter should call Nav2 costmap clear services directly.
 - Success detection is external for now. If no `succeeded` result arrives before
   a behavior's timeout, the node escalates to the next behavior and eventually
-  pauses.
+  pauses. Twist motions use their motion duration as the timeout; delegated
+  commands can use a longer completion timeout.
 - Cliff, wheel-drop, and pickup are represented as boolean topics until the
   hardware/simulation message contract is finalized.

contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/oomwoo_recovery_safety/core.py

@@ -39,6 +39,15 @@ class RecoveryStep:
     duration_sec: float
     linear_x: float = 0.0
     angular_z: float = 0.0
+    completion_timeout_sec: float | None = None
+
+    @property
+    def deadline_sec(self) -> float:
+        return (
+            self.completion_timeout_sec
+            if self.completion_timeout_sec is not None
+            else self.duration_sec
+        )
 
 
 @dataclass(frozen=True)
@@ -77,29 +86,29 @@ class Decision:
         RecoveryStep("back_up", "twist", 0.8, linear_x=-0.12),
         RecoveryStep("rotate_away_from_left_bumper", "twist", 1.0, linear_x=-0.06, angular_z=-0.55),
         RecoveryStep("wiggle_free", "twist", 0.7, linear_x=-0.04, angular_z=0.85),
-        RecoveryStep("clear_costmap", "clear_costmap", 0.1),
+        RecoveryStep("clear_costmap", "clear_costmap", 0.1, completion_timeout_sec=2.0),
     ),
     Situation.BUMPER_RIGHT: (
         RecoveryStep("back_up", "twist", 0.8, linear_x=-0.12),
         RecoveryStep("rotate_away_from_right_bumper", "twist", 1.0, linear_x=-0.06, angular_z=0.55),
         RecoveryStep("wiggle_free", "twist", 0.7, linear_x=-0.04, angular_z=-0.85),
-        RecoveryStep("clear_costmap", "clear_costmap", 0.1),
+        RecoveryStep("clear_costmap", "clear_costmap", 0.1, completion_timeout_sec=2.0),
     ),
     Situation.BUMPER_FRONT: (
         RecoveryStep("back_up", "twist", 0.9, linear_x=-0.14),
         RecoveryStep("rotate_left", "twist", 0.8, angular_z=0.6),
         RecoveryStep("rotate_right", "twist", 0.8, angular_z=-0.6),
-        RecoveryStep("clear_costmap", "clear_costmap", 0.1),
+        RecoveryStep("clear_costmap", "clear_costmap", 0.1, completion_timeout_sec=2.0),
     ),
     Situation.WEDGED: (
         RecoveryStep("back_up", "twist", 1.0, linear_x=-0.12),
         RecoveryStep("wiggle_left", "twist", 0.6, linear_x=-0.04, angular_z=0.9),
         RecoveryStep("wiggle_right", "twist", 0.6, linear_x=-0.04, angular_z=-0.9),
         RecoveryStep("rotate_in_place", "twist", 1.2, angular_z=0.7),
-        RecoveryStep("clear_costmap", "clear_costmap", 0.1),
+        RecoveryStep("clear_costmap", "clear_costmap", 0.1, completion_timeout_sec=2.0),
     ),
     Situation.NO_VALID_PATH: (
-        RecoveryStep("clear_costmap", "clear_costmap", 0.1),
+        RecoveryStep("clear_costmap", "clear_costmap", 0.1, completion_timeout_sec=2.0),
         RecoveryStep("nudge_reverse", "twist", 0.6, linear_x=-0.08),
         RecoveryStep("rotate_in_place", "twist", 1.0, angular_z=0.6),
     ),

contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/oomwoo_recovery_safety/recovery_node.py

@@ -19,6 +19,7 @@ def __init__(self):
         super().__init__("recovery_safety")
         self._controller = RecoveryController()
         self._active_deadline: float | None = None
+        self._active_twist: Twist | None = None
 
         self._cmd_pub = self.create_publisher(Twist, "cmd_vel", 10)
         self._status_pub = self.create_publisher(String, "oomwoo/status", 10)
@@ -55,51 +56,56 @@ def _behavior_result_cb(self, msg: String):
         outcome = self._parse_outcome(msg.data)
         if outcome == "succeeded":
             self._stop_motion()
-            self._active_deadline = None
+            self._clear_active_behavior()
             self._execute(self._controller.step_succeeded())
         elif outcome == "failed":
             self._stop_motion()
-            self._active_deadline = None
+            self._clear_active_behavior()
             self._execute(self._controller.step_failed("external failure result"))
         else:
             self.get_logger().warn(f"Ignoring unknown behavior outcome: {msg.data}")
 
     def _e_stop_cb(self, msg: Bool):
         if msg.data:
             self._stop_motion()
-            self._active_deadline = None
+            self._clear_active_behavior()
             self._execute(self._controller.trigger(Situation.E_STOP))
 
     def _cliff_cb(self, msg: Bool):
         if msg.data:
             self._stop_motion()
-            self._active_deadline = None
+            self._clear_active_behavior()
             self._execute(self._controller.trigger(Situation.CLIFF))
 
     def _wheel_drop_cb(self, msg: Bool):
         if msg.data:
             self._stop_motion()
-            self._active_deadline = None
+            self._clear_active_behavior()
             self._execute(self._controller.trigger(Situation.WHEEL_DROP))
 
     def _pickup_cb(self, msg: Bool):
         if msg.data:
             self._stop_motion()
-            self._active_deadline = None
+            self._clear_active_behavior()
             self._execute(self._controller.trigger(Situation.PICKUP))
 
     def _reset_cb(self, msg: Bool):
         if msg.data:
             self._stop_motion()
-            self._active_deadline = None
+            self._clear_active_behavior()
             self._execute(self._controller.reset())
 
     def _timer_cb(self):
-        if self._active_deadline is None or monotonic() < self._active_deadline:
+        if self._active_deadline is None:
+            return
+
+        if monotonic() < self._active_deadline:
+            if self._active_twist is not None:
+                self._cmd_pub.publish(self._active_twist)
             return
 
         self._stop_motion()
-        self._active_deadline = None
+        self._clear_active_behavior()
         self._execute(self._controller.step_failed("behavior timeout"))
 
     def _execute(self, decision: Decision):
@@ -112,13 +118,15 @@ def _execute(self, decision: Decision):
             twist = Twist()
             twist.linear.x = step.linear_x
             twist.angular.z = step.angular_z
+            self._active_twist = twist
             self._cmd_pub.publish(twist)
         elif step.command == "stop":
             self._stop_motion()
         else:
+            self._active_twist = None
             self._publish_command(step.command, step.name)
 
-        self._active_deadline = monotonic() + step.duration_sec
+        self._active_deadline = monotonic() + step.deadline_sec
 
     def _publish_status(self, status):
         self._status_pub.publish(String(data=status.to_json()))
@@ -132,8 +140,13 @@ def _publish_command(self, command: str, behavior: str):
         self._command_pub.publish(String(data=json.dumps(payload, sort_keys=True)))
 
     def _stop_motion(self):
+        self._active_twist = None
         self._cmd_pub.publish(Twist())
 
+    def _clear_active_behavior(self):
+        self._active_deadline = None
+        self._active_twist = None
+
     @staticmethod
     def _has_real_contact(msg: Contacts) -> bool:
         for contact in msg.contacts:

contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/test/test_recovery_controller.py

@@ -107,3 +107,13 @@ def test_status_json_shape():
     assert payload["source"] == "oomwoo_recovery_safety"
     assert payload["situation"] == "no_valid_path"
     assert payload["behavior"] == "clear_costmap"
+
+
+def test_external_steps_have_separate_completion_timeout():
+    controller = RecoveryController()
+
+    decision = controller.trigger(Situation.NO_VALID_PATH)
+
+    assert decision.step.name == "clear_costmap"
+    assert decision.step.duration_sec == 0.1
+    assert decision.step.deadline_sec == 2.0

contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/test/test_recovery_node_adapter.py

@@ -0,0 +1,161 @@
+import importlib
+import sys
+import types
+
+
+class _Vector:
+    def __init__(self):
+        self.x = 0.0
+        self.z = 0.0
+
+
+class _Twist:
+    def __init__(self):
+        self.linear = _Vector()
+        self.angular = _Vector()
+
+
+class _String:
+    def __init__(self, data=""):
+        self.data = data
+
+
+class _Bool:
+    def __init__(self, data=False):
+        self.data = data
+
+
+class _Contacts:
+    def __init__(self):
+        self.contacts = []
+
+
+class _Publisher:
+    def __init__(self):
+        self.messages = []
+
+    def publish(self, msg):
+        self.messages.append(msg)
+
+
+class _Logger:
+    def __init__(self):
+        self.warnings = []
+
+    def warn(self, message):
+        self.warnings.append(message)
+
+
+class _Node:
+    def __init__(self, name):
+        self.name = name
+        self.publishers = {}
+        self.timers = []
+        self.subscriptions = []
+        self.logger = _Logger()
+
+    def create_publisher(self, _msg_type, topic, _qos):
+        publisher = _Publisher()
+        self.publishers[topic] = publisher
+        return publisher
+
+    def create_subscription(self, msg_type, topic, callback, qos):
+        self.subscriptions.append((msg_type, topic, callback, qos))
+
+    def create_timer(self, period_sec, callback):
+        self.timers.append((period_sec, callback))
+
+    def get_logger(self):
+        return self.logger
+
+    def destroy_node(self):
+        pass
+
+
+class _ExternalShutdownException(Exception):
+    pass
+
+
+class _ROSInterruptException(Exception):
+    pass
+
+
+def _install_ros_stubs(monkeypatch):
+    geometry_msgs = types.ModuleType("geometry_msgs")
+    geometry_msgs_msg = types.ModuleType("geometry_msgs.msg")
+    geometry_msgs_msg.Twist = _Twist
+
+    rclpy = types.ModuleType("rclpy")
+    rclpy.init = lambda args=None: None
+    rclpy.spin = lambda node: None
+    rclpy.ok = lambda: False
+    rclpy.shutdown = lambda: None
+
+    rclpy_executors = types.ModuleType("rclpy.executors")
+    rclpy_executors.ExternalShutdownException = _ExternalShutdownException
+
+    rclpy_exceptions = types.ModuleType("rclpy.exceptions")
+    rclpy_exceptions.ROSInterruptException = _ROSInterruptException
+
+    rclpy_node = types.ModuleType("rclpy.node")
+    rclpy_node.Node = _Node
+
+    ros_gz_interfaces = types.ModuleType("ros_gz_interfaces")
+    ros_gz_interfaces_msg = types.ModuleType("ros_gz_interfaces.msg")
+    ros_gz_interfaces_msg.Contacts = _Contacts
+
+    std_msgs = types.ModuleType("std_msgs")
+    std_msgs_msg = types.ModuleType("std_msgs.msg")
+    std_msgs_msg.Bool = _Bool
+    std_msgs_msg.String = _String
+
+    modules = {
+        "geometry_msgs": geometry_msgs,
+        "geometry_msgs.msg": geometry_msgs_msg,
+        "rclpy": rclpy,
+        "rclpy.executors": rclpy_executors,
+        "rclpy.exceptions": rclpy_exceptions,
+        "rclpy.node": rclpy_node,
+        "ros_gz_interfaces": ros_gz_interfaces,
+        "ros_gz_interfaces.msg": ros_gz_interfaces_msg,
+        "std_msgs": std_msgs,
+        "std_msgs.msg": std_msgs_msg,
+    }
+    for name, module in modules.items():
+        monkeypatch.setitem(sys.modules, name, module)
+
+
+def _load_node_module(monkeypatch):
+    _install_ros_stubs(monkeypatch)
+    monkeypatch.delitem(sys.modules, "oomwoo_recovery_safety.recovery_node", raising=False)
+    return importlib.import_module("oomwoo_recovery_safety.recovery_node")
+
+
+def test_twist_step_is_republished_while_deadline_is_active(monkeypatch):
+    recovery_node = _load_node_module(monkeypatch)
+    node = recovery_node.RecoverySafetyNode()
+
+    node._event_cb(_String(data="wedged"))
+
+    assert len(node._cmd_pub.messages) == 1
+    held_twist = node._cmd_pub.messages[-1]
+    assert held_twist.linear.x == -0.12
+    assert held_twist.angular.z == 0.0
+
+    node._timer_cb()
+
+    assert len(node._cmd_pub.messages) == 2
+    assert node._cmd_pub.messages[-1] is held_twist
+    assert node._active_twist is held_twist
+
+
+def test_delegated_command_uses_completion_timeout(monkeypatch):
+    recovery_node = _load_node_module(monkeypatch)
+    node = recovery_node.RecoverySafetyNode()
+
+    start = recovery_node.monotonic()
+    node._event_cb(_String(data="no_valid_path"))
+
+    assert len(node._command_pub.messages) == 1
+    assert 1.5 < node._active_deadline - start <= 2.1
+    assert node._active_twist is None