From 6c0343acb02ac170397baeaca8649cd0bc761cf0 Mon Sep 17 00:00:00 2001 From: Codex Date: Mon, 7 Sep 2026 13:28:39 +0800 Subject: [PATCH 1/3] Route simulated navigation through a final velocity gate --- .github/workflows/ci.yml | 10 ++++ README.md | 17 ++++-- docs/navigation.md | 32 +++++++++++ launch/navigation_sim.launch.py | 22 +++++++- scripts/gate_smoke.py | 71 +++++++++++++++++++++++++ setup.py | 1 + tests/test_velocity_gate.py | 61 +++++++++++++++++++++ turtlebot3_multimodal/gate_node.py | 73 ++++++++++++++++++++++++++ turtlebot3_multimodal/guarded_model.py | 22 ++++++++ turtlebot3_multimodal/velocity_gate.py | 58 ++++++++++++++++++++ 10 files changed, 363 insertions(+), 4 deletions(-) create mode 100644 scripts/gate_smoke.py create mode 100644 tests/test_velocity_gate.py create mode 100644 turtlebot3_multimodal/gate_node.py create mode 100644 turtlebot3_multimodal/guarded_model.py create mode 100644 turtlebot3_multimodal/velocity_gate.py diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ec8053e..5a0cf1e 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -46,4 +46,14 @@ jobs: - run: colcon test - run: colcon test-result --verbose - run: scripts/ros_smoke.sh + - name: Final velocity gate ROS integration + run: | + source /opt/ros/humble/setup.bash + source install/setup.bash + python3 scripts/gate_smoke.py - run: scripts/navigation_smoke.sh /tmp/turtlebot3-navigation + - uses: actions/upload-artifact@v4 + if: always() + with: + name: guarded-navigation + path: /tmp/turtlebot3-navigation/ diff --git a/README.md b/README.md index 2715609..94a6f6c 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,13 @@ # Safe Multimodal TurtleBot3 +Run structured motion commands and headless Nav2 evaluation with a final +velocity gate. [Quick start](#python-verification) · [navigation](docs/navigation.md) · +[CI](https://github.com/dekaifeng/LLM-Turtlebot3/actions). + +The guarded navigation launch routes the simulated drive through `/cmd_vel_safe`. +Its final gate selects one input source, bounds velocity, and latches emergency +stop or a stale-input watchdog. See the topic contract in the navigation note. + ## Project timeline and provenance | Milestone | Date | Scope | @@ -74,7 +82,7 @@ arguments, interpretation, and limitations. ![Gazebo navigation trajectory](results/navigation/navigation_trajectory.png) -The table is one measured software-simulation run; small scheduling-dependent +The table is one measured pre-gate software-simulation run; small scheduling-dependent variation is expected. Raw odometry, metrics, the occupancy map, and the figure are committed under `results/navigation/`. @@ -85,9 +93,12 @@ flowchart LR A[Gesture / Voice / LLM / Keyboard] --> B[Strict JSON parser] B --> C[Whitelist and numeric limits] C --> D[Non-blocking MotionExecutor] - D --> E[ROS 2 /cmd_vel] - G[Gazebo / SLAM Toolbox / Nav2] --> E + D --> E[Manual input] + G[SLAM Toolbox / Nav2] --> H[Final velocity gate] + E --> H + H --> I[/cmd_vel_safe -> simulated drive] F[Watchdog / emergency-stop service] --> D + F --> H ``` ## Command example diff --git a/docs/navigation.md b/docs/navigation.md index 23c6e39..cc1c86b 100644 --- a/docs/navigation.md +++ b/docs/navigation.md @@ -1,5 +1,37 @@ # Gazebo, SLAM Toolbox, and Nav2 evaluation +## Final velocity output + +The navigation launch now changes the installed Waffle SDF's differential-drive +command topic to `/cmd_vel_safe`, retaining the other model settings. It refuses +unexpected drive plugins or topic remappings instead of silently bypassing the +gate. The temporary SDF is removed on launch shutdown. + +```text +Nav2 /cmd_vel -------------------\ + velocity_gate -> /cmd_vel_safe -> Gazebo drive +manual /turtlebot3/manual_cmd_vel / +``` + +Navigation is selected by default. `/turtlebot3/select_manual` (SetBool) selects +manual when true and navigation when false; switching clears buffered input. +`/turtlebot3/emergency_stop` latches the final gate; `/turtlebot3/reset_stop` +clears it and requires a fresh input. Input loss beyond 0.5 seconds latches a +stop, as does a timer discontinuity. The output timer uses a steady clock, so +pausing simulated time does not freeze the watchdog. The gate bounds output to +0.22 m/s and 1.5 rad/s. Unselected inputs cannot take over automatically. + +In this combined launch the manual executor's original stop/reset services are +under `/turtlebot3/manual/`. Use the final gate services to stop the drive. +The standalone `safe_controller.launch.py` retains its original interface. + +ROS CI runs `scripts/gate_smoke.py` for continuing-nav emergency stop, exclusive +source selection, and stale-input behavior, then runs guarded Gazebo navigation. +The checked-in navigation numbers predate this routing change; use the Actions +artifact for the current run. This software gate cannot stop a process that +publishes directly to `/cmd_vel_safe`, or guarantee stopping after gate/process +failure without a separate actuator-level command timeout. + Phase 2 runs the official TurtleBot3 Waffle model in Gazebo Classic, builds an occupancy grid online with SLAM Toolbox, and sends a `NavigateToPose` goal to Nav2. The evaluator subscribes to `/odom`, captures action feedback, and writes diff --git a/launch/navigation_sim.launch.py b/launch/navigation_sim.launch.py index bd14370..12f634a 100644 --- a/launch/navigation_sim.launch.py +++ b/launch/navigation_sim.launch.py @@ -1,6 +1,7 @@ """Headless TurtleBot3 world with online SLAM, Nav2, and an evaluation goal.""" import os +import tempfile from ament_index_python.packages import get_package_share_directory from launch.actions import ( @@ -9,16 +10,22 @@ IncludeLaunchDescription, RegisterEventHandler, ) -from launch.event_handlers import OnProcessExit +from launch.event_handlers import OnProcessExit, OnShutdown from launch.events import Shutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node, SetParameter from launch import LaunchDescription +from turtlebot3_multimodal.guarded_model import write_guarded_model def generate_launch_description() -> LaunchDescription: + model_directory = tempfile.TemporaryDirectory(prefix="turtlebot3-guarded-") + robot_sdf = write_guarded_model( + os.path.join(get_package_share_directory("nav2_bringup"), "worlds", "waffle.model"), + os.path.join(model_directory.name, "waffle.model"), + ) nav2_launch = os.path.join( get_package_share_directory("nav2_bringup"), "launch", "tb3_simulation_launch.py" ) @@ -53,9 +60,22 @@ def generate_launch_description() -> LaunchDescription: "slam": "True", "use_sim_time": "True", "use_composition": "False", + "robot_sdf": robot_sdf, }.items(), ), + Node(package="turtlebot3_multimodal", executable="velocity_gate", output="screen"), + Node( + package="turtlebot3_multimodal", executable="safe_controller", output="screen", + remappings=[ + ("/cmd_vel", "/turtlebot3/manual_cmd_vel"), + ("/turtlebot3/emergency_stop", "/turtlebot3/manual/emergency_stop"), + ("/turtlebot3/reset_stop", "/turtlebot3/manual/reset_stop"), + ], + ), evaluator, + RegisterEventHandler(OnShutdown( + on_shutdown=lambda event, context: model_directory.cleanup() + )), RegisterEventHandler( OnProcessExit( target_action=evaluator, diff --git a/scripts/gate_smoke.py b/scripts/gate_smoke.py new file mode 100644 index 0000000..f09f61d --- /dev/null +++ b/scripts/gate_smoke.py @@ -0,0 +1,71 @@ +"""Exercise final-gate topics and services in a real ROS graph, without a robot.""" + +import time + +import rclpy +from geometry_msgs.msg import Twist +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from std_srvs.srv import SetBool, Trigger + +from turtlebot3_multimodal.gate_node import VelocityGateNode + + +def main(): + rclpy.init() + gate, client = VelocityGateNode(), Node("velocity_gate_probe") + executor = SingleThreadedExecutor() + executor.add_node(gate) + executor.add_node(client) + received = [] + client.create_subscription(Twist, "/cmd_vel_safe", received.append, 10) + nav = client.create_publisher(Twist, "/cmd_vel", 1) + manual = client.create_publisher(Twist, "/turtlebot3/manual_cmd_vel", 1) + + def spin(duration, publisher=None, velocity=0.1): + end = time.monotonic() + duration + while time.monotonic() < end: + if publisher is not None: + message = Twist() + message.linear.x = velocity + publisher.publish(message) + executor.spin_once(timeout_sec=0.01) + + def service(name, kind, request): + connection = client.create_client(kind, name) + assert connection.wait_for_service(timeout_sec=5), name + future = connection.call_async(request) + executor.spin_until_future_complete(future, timeout_sec=5) + assert future.done() and future.result().success, name + client.destroy_client(connection) + spin(0.1) + received.clear() + + try: + spin(1.0, nav) + assert any(message.linear.x > 0 for message in received), "nav did not reach final output" + service("/turtlebot3/emergency_stop", Trigger, Trigger.Request()) + spin(0.2, nav) + assert received and all(message.linear.x == 0 for message in received), "stop bypassed" + service("/turtlebot3/reset_stop", Trigger, Trigger.Request()) + request = SetBool.Request() + request.data = True + service("/turtlebot3/select_manual", SetBool, request) + spin(0.2, nav) + assert received and all(message.linear.x == 0 for message in received), "source conflict" + spin(0.2, manual, -0.1) + assert any(message.linear.x < 0 for message in received), "manual source did not move" + spin(0.7) + received.clear() + spin(0.2, manual, -0.1) + assert received and all(message.linear.x == 0 for message in received), "watchdog not latched" + print("gate ROS smoke: navigation, emergency stop, source isolation, watchdog passed") + finally: + executor.shutdown() + gate.destroy_node() + client.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/setup.py b/setup.py index f5fa4ab..911b2f7 100644 --- a/setup.py +++ b/setup.py @@ -24,6 +24,7 @@ entry_points={ "console_scripts": [ "safe_controller = turtlebot3_multimodal.ros_node:main", + "velocity_gate = turtlebot3_multimodal.gate_node:main", "evaluate_commands = turtlebot3_multimodal.evaluate_cli:main", "navigation_evaluator = turtlebot3_multimodal.navigation_evaluator:main", ], diff --git a/tests/test_velocity_gate.py b/tests/test_velocity_gate.py new file mode 100644 index 0000000..53ae96c --- /dev/null +++ b/tests/test_velocity_gate.py @@ -0,0 +1,61 @@ +import pytest + +from turtlebot3_multimodal.executor import Velocity +from turtlebot3_multimodal.guarded_model import write_guarded_model +from turtlebot3_multimodal.velocity_gate import VelocityGate + + +def test_estop_blocks_continuing_navigation_until_reset_and_fresh_input(): + gate = VelocityGate() + gate.receive("nav", Velocity(0.1), 0) + assert gate.tick(0) == Velocity(0.1) + gate.stop() + gate.receive("nav", Velocity(0.2), 0.1) + assert gate.tick(0.1) == Velocity() + gate.reset() + assert gate.tick(0.2) == Velocity() + gate.receive("nav", Velocity(0.1), 0.3) + assert gate.tick(0.3) == Velocity(0.1) + + +def test_exclusive_source_selection_drops_old_commands_and_limits_speed(): + gate = VelocityGate() + gate.receive("manual", Velocity(-0.1), 0) + assert gate.tick(0) == Velocity() + gate.receive("nav", Velocity(9, 9), 0.1) + assert gate.tick(0.1) == Velocity(0.22, 1.5) + gate.select("manual") + assert gate.tick(0.2) == Velocity() + gate.receive("nav", Velocity(0.2), 0.2) + gate.receive("manual", Velocity(-0.1), 0.2) + assert gate.tick(0.2) == Velocity(-0.1) + + +def test_input_watchdog_latches_stop_even_when_output_timer_is_alive(): + gate = VelocityGate() + gate.receive("nav", Velocity(0.1), 0) + for tick in (0, 0.2, 0.4): + assert gate.tick(tick) == Velocity(0.1) + assert gate.tick(0.6) == Velocity() + gate.receive("nav", Velocity(0.1), 0.7) + assert gate.tick(0.7) == Velocity() + + +@pytest.mark.parametrize("time", [float("nan"), -1, 1]) +def test_invalid_or_discontinuous_output_clock_stops(time): + gate = VelocityGate() + gate.receive("nav", Velocity(0.1), 0) + gate.tick(0) + assert gate.tick(time) == Velocity() + assert gate.stopped + + +def test_model_routing_fails_closed_for_unexpected_models(tmp_path): + source, target = tmp_path / "in.sdf", tmp_path / "out.sdf" + source.write_text('' + 'cmd_vel') + write_guarded_model(str(source), str(target)) + assert "/cmd_vel_safe" in target.read_text() + source.write_text("") + with pytest.raises(ValueError, match="exactly one"): + write_guarded_model(str(source), str(target)) diff --git a/turtlebot3_multimodal/gate_node.py b/turtlebot3_multimodal/gate_node.py new file mode 100644 index 0000000..07618ad --- /dev/null +++ b/turtlebot3_multimodal/gate_node.py @@ -0,0 +1,73 @@ +"""ROS adapter for the final velocity gate; uses a steady watchdog clock.""" + +import time + +import rclpy +from geometry_msgs.msg import Twist +from rclpy.clock import Clock, ClockType +from rclpy.node import Node +from std_srvs.srv import SetBool, Trigger + +from turtlebot3_multimodal.executor import Velocity +from turtlebot3_multimodal.velocity_gate import VelocityGate + + +class VelocityGateNode(Node): + def __init__(self): + super().__init__("velocity_gate") + self.gate = VelocityGate() + self.publisher = self.create_publisher(Twist, "/cmd_vel_safe", 1) + self.create_subscription(Twist, "/cmd_vel", self.nav, 1) + self.create_subscription(Twist, "/turtlebot3/manual_cmd_vel", self.manual, 1) + self.create_service(Trigger, "/turtlebot3/emergency_stop", self.stop) + self.create_service(Trigger, "/turtlebot3/reset_stop", self.reset) + self.create_service(SetBool, "/turtlebot3/select_manual", self.select) + self.create_timer(0.02, self.publish, clock=Clock(clock_type=ClockType.STEADY_TIME)) + + def nav(self, message): + self.receive("nav", message) + + def manual(self, message): + self.receive("manual", message) + + def receive(self, source, message): + self.gate.receive(source, Velocity(message.linear.x, message.angular.z), time.monotonic()) + + def publish(self): + velocity = self.gate.tick(time.monotonic()) + message = Twist() + message.linear.x, message.angular.z = velocity.linear_x, velocity.angular_z + self.publisher.publish(message) + + def stop(self, request, response): + self.gate.stop() + self.publish() + response.success, response.message = True, "final velocity stop latched" + return response + + def reset(self, request, response): + self.gate.reset() + response.success, response.message = True, "reset; fresh selected-source input required" + return response + + def select(self, request, response): + self.gate.select("manual" if request.data else "nav") + self.publish() + response.success, response.message = True, self.gate.source + return response + + +def main(args=None): + rclpy.init(args=args) + node = VelocityGateNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.gate.stop() + if rclpy.ok(): + node.publish() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() diff --git a/turtlebot3_multimodal/guarded_model.py b/turtlebot3_multimodal/guarded_model.py new file mode 100644 index 0000000..fed491a --- /dev/null +++ b/turtlebot3_multimodal/guarded_model.py @@ -0,0 +1,22 @@ +"""Route the Gazebo differential-drive plugin through the final gate.""" + +import xml.etree.ElementTree as ET +from pathlib import Path + + +def write_guarded_model(source: str, destination: str) -> str: + tree = ET.parse(source) + plugins = [p for p in tree.iter("plugin") if p.get("filename") == "libgazebo_ros_diff_drive.so"] + if len(plugins) != 1: + raise ValueError("expected exactly one Gazebo ROS differential-drive plugin") + plugin = plugins[0] + command = plugin.find("command_topic") + if command is None or command.text not in {"cmd_vel", "/cmd_vel"}: + raise ValueError("unexpected drive command topic; inspect the installed Nav2 model") + for remap in plugin.findall("ros/remapping"): + if "cmd_vel" in (remap.text or ""): + raise ValueError("unexpected velocity remapping in source model") + command.text = "/cmd_vel_safe" + Path(destination).parent.mkdir(parents=True, exist_ok=True) + tree.write(destination, encoding="utf-8", xml_declaration=True) + return destination diff --git a/turtlebot3_multimodal/velocity_gate.py b/turtlebot3_multimodal/velocity_gate.py new file mode 100644 index 0000000..32d3c10 --- /dev/null +++ b/turtlebot3_multimodal/velocity_gate.py @@ -0,0 +1,58 @@ +"""Exclusive source selection and a latched stop at the final velocity output.""" + +import math + +from turtlebot3_multimodal.commands import SafetyLimits +from turtlebot3_multimodal.executor import Velocity + + +class VelocityGate: + def __init__(self, limits: SafetyLimits | None = None): + self.limits = limits or SafetyLimits() + self.source = "nav" + self.stopped = False + self._sample: tuple[Velocity, float] | None = None + self._last_tick: float | None = None + + def select(self, source: str) -> None: + if source not in {"nav", "manual"}: + raise ValueError("source must be nav or manual") + self.source = source + self._sample = None + + def stop(self) -> None: + self.stopped = True + self._sample = None + + def reset(self) -> None: + self.stopped = False + self._sample = None + self._last_tick = None + + def receive(self, source: str, velocity: Velocity, now: float) -> None: + if source != self.source or self.stopped: + return + if not all(math.isfinite(x) for x in (velocity.linear_x, velocity.angular_z, now)): + self.stop() + return + self._sample = (Velocity( + max(-self.limits.max_linear_mps, min(self.limits.max_linear_mps, velocity.linear_x)), + max(-self.limits.max_angular_rps, min(self.limits.max_angular_rps, velocity.angular_z)), + ), now) + + def tick(self, now: float) -> Velocity: + if not math.isfinite(now) or ( + self._last_tick is not None and ( + now < self._last_tick + or now - self._last_tick > self.limits.watchdog_timeout_s + ) + ): + self.stop() + self._last_tick = now + if self.stopped or self._sample is None: + return Velocity() + velocity, received = self._sample + if not 0 <= now - received <= self.limits.watchdog_timeout_s: + self.stop() + return Velocity() + return velocity From a2ec57476f32993a7325cfdf6a13bdb37a0763ab Mon Sep 17 00:00:00 2001 From: Codex Date: Mon, 7 Sep 2026 13:39:00 +0800 Subject: [PATCH 2/3] Keep generated SDF compatible with Humble spawning --- tests/test_velocity_gate.py | 1 + turtlebot3_multimodal/guarded_model.py | 3 ++- turtlebot3_multimodal/ros_node.py | 6 ++++-- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/tests/test_velocity_gate.py b/tests/test_velocity_gate.py index 53ae96c..87e1a45 100644 --- a/tests/test_velocity_gate.py +++ b/tests/test_velocity_gate.py @@ -56,6 +56,7 @@ def test_model_routing_fails_closed_for_unexpected_models(tmp_path): 'cmd_vel') write_guarded_model(str(source), str(target)) assert "/cmd_vel_safe" in target.read_text() + assert "") with pytest.raises(ValueError, match="exactly one"): write_guarded_model(str(source), str(target)) diff --git a/turtlebot3_multimodal/guarded_model.py b/turtlebot3_multimodal/guarded_model.py index fed491a..90b2af1 100644 --- a/turtlebot3_multimodal/guarded_model.py +++ b/turtlebot3_multimodal/guarded_model.py @@ -18,5 +18,6 @@ def write_guarded_model(source: str, destination: str) -> str: raise ValueError("unexpected velocity remapping in source model") command.text = "/cmd_vel_safe" Path(destination).parent.mkdir(parents=True, exist_ok=True) - tree.write(destination, encoding="utf-8", xml_declaration=True) + # Humble spawn_entity passes Unicode to lxml, which rejects encoding declarations. + tree.write(destination, encoding="utf-8", xml_declaration=False) return destination diff --git a/turtlebot3_multimodal/ros_node.py b/turtlebot3_multimodal/ros_node.py index ee50f5b..a82429e 100644 --- a/turtlebot3_multimodal/ros_node.py +++ b/turtlebot3_multimodal/ros_node.py @@ -83,6 +83,8 @@ def main(args=None) -> None: except KeyboardInterrupt: pass finally: - node._publish(Velocity()) + if rclpy.ok(): + node._publish(Velocity()) node.destroy_node() - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() From 91411286db7af85cf496b64d0cea3dcd39b396a4 Mon Sep 17 00:00:00 2001 From: Codex Date: Mon, 7 Sep 2026 13:55:41 +0800 Subject: [PATCH 3/3] Retain CI environment evidence and improve verification portability --- .github/workflows/ci.yml | 34 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5a0cf1e..204273b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -5,6 +5,12 @@ on: branches: [main] pull_request: +permissions: + contents: read + +env: + MPLBACKEND: Agg + jobs: python: runs-on: ubuntu-22.04 @@ -23,6 +29,20 @@ jobs: - run: pytest -q - run: turtlebot-command-evaluation --output-dir /tmp/turtlebot3-evaluation + - name: Retain run environment + if: always() + run: | + mkdir -p ci-evidence + python --version > ci-evidence/python.txt + python -m pip freeze > ci-evidence/requirements.txt + git rev-parse HEAD > ci-evidence/git-sha.txt + uname -a > ci-evidence/host.txt + - uses: actions/upload-artifact@v4 + if: always() + with: + name: environment-python-${{ matrix.python-version }} + path: ci-evidence/ + retention-days: 14 ros2-humble: runs-on: ubuntu-22.04 steps: @@ -57,3 +77,17 @@ jobs: with: name: guarded-navigation path: /tmp/turtlebot3-navigation/ + - name: Retain run environment + if: always() + run: | + mkdir -p ci-evidence + python --version > ci-evidence/python.txt + python -m pip freeze > ci-evidence/requirements.txt + git rev-parse HEAD > ci-evidence/git-sha.txt + uname -a > ci-evidence/host.txt + - uses: actions/upload-artifact@v4 + if: always() + with: + name: environment-ros2-humble + path: ci-evidence/ + retention-days: 14