agent update!

ros_bridge/bridge.py

@@ -25,7 +25,7 @@ from rclpy.executors import SingleThreadedExecutor
 from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy
 from rclpy.action import ActionClient
 
-from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
+from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped, Twist
 from lifecycle_msgs.srv import GetState
 from nav_msgs.msg import OccupancyGrid, Odometry, Path as NavPath
 from nav_msgs.srv import GetMap
@@ -237,6 +237,13 @@ class SubscriberNode(Node):
             "/rviz/routing/rough_goal",
             10,
         )
+        self._cmd_vel_pub = None
+        if _ROBOT_STACK == "nav2":
+            self._cmd_vel_pub = self.create_publisher(
+                Twist,
+                "/cmd_vel",
+                10,
+            )
 
         # Short control trajectory / long predicted path
         self._trajectory = None
@@ -1247,6 +1254,17 @@ class SubscriberNode(Node):
         self._nav2_goal_handle = None
         return {"ok": True}
 
+    def publish_nav2_stop(self, repeat=1, interval=0.1):
+        if self._cmd_vel_pub is None:
+            return {"topic": "/cmd_vel", "published": 0}
+
+        zero_velocity = Twist()
+        for index in range(max(1, repeat)):
+            self._cmd_vel_pub.publish(zero_velocity)
+            if index + 1 < repeat:
+                time.sleep(interval)
+        return {"topic": "/cmd_vel", "published": max(1, repeat)}
+
 
 sub_node: Optional[SubscriberNode] = None
 _spin_thread: Optional[threading.Thread] = None
@@ -1676,6 +1694,37 @@ def disengage():
         raise HTTPException(status_code=500, detail=str(e))
 
 
+@app.post("/emergency_stop")
+def emergency_stop():
+    """Stop motion immediately through the active robot stack.
+
+    This is a software stop. A physical emergency stop still has to be handled
+    by the platform's safety controller.
+    """
+    try:
+        if _ROBOT_STACK == "nav2":
+            # Stop the controller first, then cancel Nav2 so a pending action
+            # cannot issue another velocity command while cancellation completes.
+            initial_stop = sub_node.publish_nav2_stop()
+            cancel_result = sub_node.cancel_nav2_goal()
+            repeated_stop = sub_node.publish_nav2_stop(repeat=4)
+            return {
+                "ok": True,
+                "stack": "nav2",
+                "cancelled": cancel_result.get("ok", False),
+                "cmdVel": {
+                    "topic": initial_stop["topic"],
+                    "published": initial_stop["published"] + repeated_stop["published"],
+                },
+            }
+
+        result = disengage()
+        return {**result, "stack": _ROBOT_STACK, "emergency": True}
+    except Exception as e:
+        logging.error(traceback.format_exc())
+        raise HTTPException(status_code=500, detail=str(e))
+
+
 @app.post("/reset")
 def reset():
     """