add lifecycle node conversion and foxglove utilities

- Convert all 7 SLAM nodes to lifecycle nodes (FASTLIO2 + arise_slam)
- Add handle_once=True to prevent re-activation loops
- Add foxglove relay scripts (twist, goal/autonomy)
- Update Docker and launch files

Author: iserverobotics-bona

docker/.env

@@ -57,6 +57,18 @@ USE_RVIZ=false
 # Set to file prefix (no .pcd extension), e.g., /ros2_ws/maps/warehouse
 MAP_PATH=
 
+# ============================================
+# ROS Bridge Server (TCP API for programmatic control)
+# ============================================
+# Bind address (0.0.0.0 = all interfaces)
+#ROS_BRIDGE_HOST=0.0.0.0
+
+# TCP port for the bridge API
+#ROS_BRIDGE_PORT=9090
+
+# Default autonomy speed (0.0 to 1.0)
+#ROS_BRIDGE_AUTONOMY_SPEED=1.0
+
 # ============================================
 # Device Group IDs
 # ============================================

docker/Dockerfile

@@ -210,6 +210,11 @@ COPY system_bagfile.sh ${WORKSPACE}/
 COPY system_bagfile_with_route_planner.sh ${WORKSPACE}/
 COPY system_bagfile_with_exploration_planner.sh ${WORKSPACE}/
 
+# Copy foxglove relay scripts
+COPY docker/foxglove_utility/twist_relay.py /usr/local/bin/twist_relay.py
+COPY docker/foxglove_utility/goal_autonomy_relay.py /usr/local/bin/goal_autonomy_relay.py
+RUN chmod +x /usr/local/bin/twist_relay.py /usr/local/bin/goal_autonomy_relay.py
+
 # Create directories for maps and logs
 RUN mkdir -p ${WORKSPACE}/maps ${WORKSPACE}/logs
 
@@ -270,6 +275,10 @@ if [ "${ENABLE_WIFI_BUFFER}" = "true" ]; then
     sysctl -w net.core.wmem_max=67108864 net.core.wmem_default=67108864 2>/dev/null || true
 fi
 
+# Launch foxglove relay scripts in background
+python3 /usr/local/bin/twist_relay.py &
+python3 /usr/local/bin/goal_autonomy_relay.py &
+
 exec "$@"
 ENTRYPOINT_EOF
 RUN chmod +x /ros_entrypoint.sh

docker/docker-compose.yml

@@ -41,6 +41,10 @@ services:
       - ENABLE_WIFI_BUFFER=${ENABLE_WIFI_BUFFER:-false}
       - USE_RVIZ=${USE_RVIZ:-false}
       - MAP_PATH=${MAP_PATH:-}
+      # ROS Bridge Server (TCP API for programmatic control, port 9090)
+      - ROS_BRIDGE_HOST=${ROS_BRIDGE_HOST:-0.0.0.0}
+      - ROS_BRIDGE_PORT=${ROS_BRIDGE_PORT:-9090}
+      - ROS_BRIDGE_AUTONOMY_SPEED=${ROS_BRIDGE_AUTONOMY_SPEED:-1.0}
 
     volumes:
       # X11 for GUI (RViz)
@@ -72,7 +76,14 @@ services:
       - -c
       - |
         echo "Starting real robot system with route planner..."
-        ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py use_fastlio2:=true &
+        echo "  ROS Bridge Server: port $${ROS_BRIDGE_PORT:-9090} (TCP API)"
+        echo "  Foxglove Bridge:   port 8765 (WebSocket)"
+        ros2 launch vehicle_simulator system_real_robot_with_route_planner.launch.py \
+          use_fastlio2:=true \
+          enable_bridge:=true \
+          bridge_host:=$${ROS_BRIDGE_HOST:-0.0.0.0} \
+          bridge_port:=$${ROS_BRIDGE_PORT:-9090} \
+          bridge_autonomy_speed:=$${ROS_BRIDGE_AUTONOMY_SPEED:-1.0} &
         NAV_PID=$$!
         sleep 2
         echo "Starting Foxglove Bridge on port 8765..."

docker/fix.txt

@@ -0,0 +1,50 @@
+# Fixes needed for nav_autonomy Docker image
+# Apply these changes to the Dockerfile, then rebuild and push
+
+# ============================================================================
+# FIX 1: Add unitree_webrtc_connect to Dockerfile
+# ============================================================================
+# Add after line ~357 (after portaudio19-dev install):
+
+# Install unitree_webrtc_connect for Unitree Go2 robot control
+# Clone, fix missing __init__.py files (upstream bug), then install
+RUN cd /tmp && \
+    git clone https://github.com/VectorRobotics/unitree_webrtc_connect.git && \
+    touch /tmp/unitree_webrtc_connect/go2_webrtc_driver/msgs/__init__.py && \
+    touch /tmp/unitree_webrtc_connect/go2_webrtc_driver/lidar/__init__.py && \
+    pip3 install --break-system-packages /tmp/unitree_webrtc_connect && \
+    rm -rf /tmp/unitree_webrtc_connect
+
+# ============================================================================
+# FIX 2: Add relay scripts to Dockerfile
+# ============================================================================
+# Add after the COPY commands for config files (around line ~387):
+
+COPY docker/navigation/foxglove_utility/twist_relay.py /usr/local/bin/twist_relay.py
+COPY docker/navigation/foxglove_utility/goal_autonomy_relay.py /usr/local/bin/goal_autonomy_relay.py
+RUN chmod +x /usr/local/bin/twist_relay.py /usr/local/bin/goal_autonomy_relay.py
+
+# ============================================================================
+# FIX 3: Ensure pip3 is available for installing Python packages
+# ============================================================================
+# Add to the apt-get install section (around line ~25):
+
+python3-pip \
+
+# ============================================================================
+# Summary of files to modify:
+# ============================================================================
+# 1. docker/navigation/Dockerfile - add the above changes
+# 2. Ensure docker/navigation/foxglove_utility/twist_relay.py exists
+# 3. Ensure docker/navigation/foxglove_utility/goal_autonomy_relay.py exists
+
+# ============================================================================
+# Rebuild commands (run from dimos repo root):
+# ============================================================================
+# cd /path/to/dimos
+# docker compose -f docker/navigation/docker-compose.yml build
+# docker tag dimos_autonomy_stack:jazzy iserverobotics/nav_autonomy:latest
+# docker push iserverobotics/nav_autonomy:latest
+
+# Or for multi-arch:
+# docker buildx build --platform linux/arm64 -t iserverobotics/nav_autonomy:latest --push -f docker/navigation/Dockerfile .

docker/foxglove_utility/goal_autonomy_relay.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+"""
+Relay node that publishes Joy message to enable autonomy mode when goal_pose is received.
+Mimics the behavior of the goalpoint_rviz_plugin for Foxglove compatibility.
+"""
+
+from geometry_msgs.msg import PointStamped, PoseStamped
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
+from sensor_msgs.msg import Joy
+
+
+class GoalAutonomyRelay(Node):
+    def __init__(self):
+        super().__init__("goal_autonomy_relay")
+
+        # QoS for goal topics (match foxglove_bridge)
+        goal_qos = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST,
+            durability=DurabilityPolicy.VOLATILE,
+            depth=5,
+        )
+
+        # Subscribe to goal_pose (PoseStamped from Foxglove)
+        self.pose_sub = self.create_subscription(
+            PoseStamped, "/goal_pose", self.goal_pose_callback, goal_qos
+        )
+
+        # Subscribe to way_point (PointStamped from Foxglove)
+        self.point_sub = self.create_subscription(
+            PointStamped, "/way_point", self.way_point_callback, goal_qos
+        )
+
+        # Publisher for Joy message to enable autonomy
+        self.joy_pub = self.create_publisher(Joy, "/joy", 5)
+
+        self.get_logger().info(
+            "Goal autonomy relay started - will publish Joy to enable autonomy when goals are received"
+        )
+
+    def publish_autonomy_joy(self):
+        """Publish Joy message that enables autonomy mode (mimics goalpoint_rviz_plugin)"""
+        joy = Joy()
+        joy.header.stamp = self.get_clock().now().to_msg()
+        joy.header.frame_id = "goal_autonomy_relay"
+
+        # axes[2] = -1.0 enables autonomy mode in pathFollower
+        # axes[4] = 1.0 sets forward speed
+        # axes[5] = 1.0 enables obstacle checking
+        joy.axes = [0.0, 0.0, -1.0, 0.0, 1.0, 1.0, 0.0, 0.0]
+
+        # buttons[7] = 1 (same as RViz plugin)
+        joy.buttons = [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0]
+
+        self.joy_pub.publish(joy)
+        self.get_logger().info("Published Joy message to enable autonomy mode")
+
+    def goal_pose_callback(self, msg: PoseStamped):
+        self.get_logger().info(
+            f"Received goal_pose at ({msg.pose.position.x:.2f}, {msg.pose.position.y:.2f})"
+        )
+        self.publish_autonomy_joy()
+
+    def way_point_callback(self, msg: PointStamped):
+        self.get_logger().info(f"Received way_point at ({msg.point.x:.2f}, {msg.point.y:.2f})")
+        self.publish_autonomy_joy()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = GoalAutonomyRelay()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

docker/foxglove_utility/twist_relay.py

@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+"""
+Simple relay node that converts geometry_msgs/Twist to geometry_msgs/TwistStamped.
+Used for Foxglove Teleop panel which only publishes Twist.
+"""
+
+from geometry_msgs.msg import Twist, TwistStamped
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+
+
+class TwistRelay(Node):
+    def __init__(self):
+        super().__init__("twist_relay")
+
+        # Declare parameters
+        self.declare_parameter("input_topic", "/foxglove_teleop")
+        self.declare_parameter("output_topic", "/cmd_vel")
+        self.declare_parameter("frame_id", "vehicle")
+
+        input_topic = self.get_parameter("input_topic").value
+        output_topic = self.get_parameter("output_topic").value
+        self.frame_id = self.get_parameter("frame_id").value
+
+        # QoS for real-time control
+        qos = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, depth=1
+        )
+
+        # Subscribe to Twist (from Foxglove Teleop)
+        self.subscription = self.create_subscription(Twist, input_topic, self.twist_callback, qos)
+
+        # Publish TwistStamped
+        self.publisher = self.create_publisher(TwistStamped, output_topic, qos)
+
+        self.get_logger().info(
+            f"Twist relay: {input_topic} (Twist) -> {output_topic} (TwistStamped)"
+        )
+
+    def twist_callback(self, msg: Twist):
+        stamped = TwistStamped()
+        stamped.header.stamp = self.get_clock().now().to_msg()
+        stamped.header.frame_id = self.frame_id
+        stamped.twist = msg
+        self.publisher.publish(stamped)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = TwistRelay()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/base_autonomy/vehicle_simulator/launch/system_bagfile.launch.py

@@ -18,6 +18,10 @@ def generate_launch_description():
   vehicleX = LaunchConfiguration('vehicleX')
   vehicleY = LaunchConfiguration('vehicleY')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  enable_bridge = LaunchConfiguration('enable_bridge')
+  bridge_host = LaunchConfiguration('bridge_host')
+  bridge_port = LaunchConfiguration('bridge_port')
+  bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed')
 
   declare_use_sim_time = DeclareLaunchArgument(
     'use_sim_time',
@@ -36,6 +40,10 @@ def generate_launch_description():
   declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='')
   declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server')
+  declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address')
+  declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port')
+  declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -107,6 +115,19 @@ def generate_launch_description():
   		}]
   )
 
+  start_ros_bridge = Node(
+    package='ros_bridge_server',
+    executable='ros_bridge_server',
+    name='ros_bridge_server',
+    output='screen',
+    arguments=[
+      '--host', bridge_host,
+      '--port', bridge_port,
+      '--autonomy-speed', bridge_autonomy_speed,
+    ],
+    condition=IfCondition(enable_bridge),
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -120,6 +141,10 @@ def generate_launch_description():
   ld.add_action(declare_vehicleX)
   ld.add_action(declare_vehicleY)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_enable_bridge)
+  ld.add_action(declare_bridge_host)
+  ld.add_action(declare_bridge_port)
+  ld.add_action(declare_bridge_autonomy_speed)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)
@@ -129,5 +154,6 @@ def generate_launch_description():
   ld.add_action(start_fastlio2)
   ld.add_action(start_visualization_tools)
   ld.add_action(start_joy)
+  ld.add_action(start_ros_bridge)
 
   return ld

src/base_autonomy/vehicle_simulator/launch/system_bagfile_with_exploration_planner.launch.py

@@ -19,6 +19,10 @@ def generate_launch_description():
   vehicleX = LaunchConfiguration('vehicleX')
   vehicleY = LaunchConfiguration('vehicleY')
   checkTerrainConn = LaunchConfiguration('checkTerrainConn')
+  enable_bridge = LaunchConfiguration('enable_bridge')
+  bridge_host = LaunchConfiguration('bridge_host')
+  bridge_port = LaunchConfiguration('bridge_port')
+  bridge_autonomy_speed = LaunchConfiguration('bridge_autonomy_speed')
 
   declare_use_sim_time = DeclareLaunchArgument(
     'use_sim_time',
@@ -38,6 +42,10 @@ def generate_launch_description():
   declare_vehicleX = DeclareLaunchArgument('vehicleX', default_value='0.0', description='')
   declare_vehicleY = DeclareLaunchArgument('vehicleY', default_value='0.0', description='')
   declare_checkTerrainConn = DeclareLaunchArgument('checkTerrainConn', default_value='true', description='')
+  declare_enable_bridge = DeclareLaunchArgument('enable_bridge', default_value='true', description='Enable ROS bridge server')
+  declare_bridge_host = DeclareLaunchArgument('bridge_host', default_value='0.0.0.0', description='ROS bridge bind address')
+  declare_bridge_port = DeclareLaunchArgument('bridge_port', default_value='9090', description='ROS bridge port')
+  declare_bridge_autonomy_speed = DeclareLaunchArgument('bridge_autonomy_speed', default_value='1.0', description='Default autonomy speed')
 
   start_local_planner = IncludeLaunchDescription(
     PythonLaunchDescriptionSource(os.path.join(
@@ -117,6 +125,19 @@ def generate_launch_description():
     }.items()
   )
 
+  start_ros_bridge = Node(
+    package='ros_bridge_server',
+    executable='ros_bridge_server',
+    name='ros_bridge_server',
+    output='screen',
+    arguments=[
+      '--host', bridge_host,
+      '--port', bridge_port,
+      '--autonomy-speed', bridge_autonomy_speed,
+    ],
+    condition=IfCondition(enable_bridge),
+  )
+
   ld = LaunchDescription()
 
   # Add the actions
@@ -131,6 +152,10 @@ def generate_launch_description():
   ld.add_action(declare_vehicleX)
   ld.add_action(declare_vehicleY)
   ld.add_action(declare_checkTerrainConn)
+  ld.add_action(declare_enable_bridge)
+  ld.add_action(declare_bridge_host)
+  ld.add_action(declare_bridge_port)
+  ld.add_action(declare_bridge_autonomy_speed)
 
   ld.add_action(start_local_planner)
   ld.add_action(start_terrain_analysis)
@@ -141,5 +166,6 @@ def generate_launch_description():
   ld.add_action(start_visualization_tools)
   ld.add_action(start_joy)
   ld.add_action(start_tare_planner)
+  ld.add_action(start_ros_bridge)
 
   return ld