diff --git a/contributions/recovery-safety/xbattlax/README.md b/contributions/recovery-safety/xbattlax/README.md index 53c6378..3ed3aaf 100644 --- a/contributions/recovery-safety/xbattlax/README.md +++ b/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. diff --git a/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/oomwoo_recovery_safety/core.py b/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/oomwoo_recovery_safety/core.py index d50f1f8..7bd5fab 100644 --- a/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/oomwoo_recovery_safety/core.py +++ b/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), ), diff --git a/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/oomwoo_recovery_safety/recovery_node.py b/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/oomwoo_recovery_safety/recovery_node.py index 1fbe6ea..1f4d048 100644 --- a/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/oomwoo_recovery_safety/recovery_node.py +++ b/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,11 +56,11 @@ 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}") @@ -67,39 +68,44 @@ def _behavior_result_cb(self, msg: String): 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: diff --git a/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/test/test_recovery_controller.py b/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/test/test_recovery_controller.py index 03477fd..21bd91c 100644 --- a/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/test/test_recovery_controller.py +++ b/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 diff --git a/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/test/test_recovery_node_adapter.py b/contributions/recovery-safety/xbattlax/oomwoo_recovery_safety/test/test_recovery_node_adapter.py new file mode 100644 index 0000000..ef6782b --- /dev/null +++ b/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