Route navigation through a final velocity gate

.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:
@@ -46,4 +66,28 @@ 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/
+      - 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

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

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

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,

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()

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",
         ],

tests/test_velocity_gate.py

@@ -0,0 +1,62 @@
+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('<sdf><plugin filename="libgazebo_ros_diff_drive.so">'
+                      '<command_topic>cmd_vel</command_topic></plugin></sdf>')
+    write_guarded_model(str(source), str(target))
+    assert "/cmd_vel_safe" in target.read_text()
+    assert "<?xml" not in target.read_text()
+    source.write_text("<sdf/>")
+    with pytest.raises(ValueError, match="exactly one"):
+        write_guarded_model(str(source), str(target))