""" full_stack.launch.py — One-command full autonomous stack bringup for SaltyBot. Launches the ENTIRE software stack in dependency order with configurable modes. Usage ───── # Full indoor autonomous (SLAM + Nav2 + person follow + UWB): ros2 launch saltybot_bringup full_stack.launch.py # Person-follow only (no SLAM, no Nav2 — living room demo): ros2 launch saltybot_bringup full_stack.launch.py mode:=follow # Outdoor GPS nav + person follow: ros2 launch saltybot_bringup full_stack.launch.py mode:=outdoor # Indoor, no CSI cameras (RealSense + RPLIDAR only): ros2 launch saltybot_bringup full_stack.launch.py mode:=indoor enable_csi_cameras:=false # Simulation / rosbag replay: ros2 launch saltybot_bringup full_stack.launch.py use_sim_time:=true enable_bridge:=false Modes ───── indoor (default) ─ RPLIDAR + RealSense D435i sensors ─ RTAB-Map SLAM (RGB-D + LIDAR, fresh map each boot) ─ Nav2 navigation stack (planners, controllers, BT navigator) ─ CSI 4× IMX219 surround cameras (optional, default on) ─ UWB driver (2-anchor DW3000, publishes /uwb/target) ─ YOLOv8n person detection (TensorRT) ─ Person follower with UWB+camera fusion ─ cmd_vel bridge → STM32 (deadman + ramp + AUTONOMOUS gate) ─ rosbridge WebSocket (port 9090) outdoor ─ RPLIDAR + RealSense D435i sensors (no SLAM) ─ GPS-based outdoor navigation (robot_localization EKF + Nav2) ─ UWB + person detection + follower ─ cmd_vel bridge + rosbridge follow ─ RealSense D435i + RPLIDAR only (no SLAM, no Nav2) ─ UWB + person detection + follower ─ cmd_vel bridge + rosbridge ─ Note: obstacle avoidance disabled (no Nav2 local costmap) Launch sequence (wall-clock delays — conservative for cold start) ───────────────────────────────────────────────────────────────── t= 0s robot_description (URDF + TF tree) t= 0s STM32 bridge (serial port owner — must be first) t= 2s cmd_vel bridge (consumes /cmd_vel, needs STM32 bridge up) t= 2s sensors (RPLIDAR + RealSense) t= 4s UWB driver (independent serial device) t= 4s CSI cameras (optional, independent) t= 6s SLAM / outdoor nav (needs sensors; indoor/outdoor mode only) t= 6s person detection (needs RealSense up) t=14s Nav2 (needs SLAM to have partial map; indoor only) t= 9s person follower (needs perception + UWB) t=17s rosbridge (last — exposes all topics over WebSocket) Safety wiring ───────────── • STM32 bridge must be up before cmd_vel bridge sends any command. • cmd_vel bridge has 500ms deadman: stops robot if /cmd_vel goes silent. • STM32 AUTONOMOUS mode gate (md=2) in cmd_vel bridge — robot stays still until STM32 firmware is in AUTONOMOUS mode regardless of /cmd_vel. • follow_enabled:=false disables person follower without stopping the node. • To e-stop at runtime: ros2 topic pub /saltybot/estop std_msgs/Bool '{data: true}' Topics published by this stack ─────────────────────────────── /scan RPLIDAR LaserScan /camera/color/image_raw RealSense RGB /camera/depth/image_rect_raw RealSense depth /camera/imu RealSense IMU /rtabmap/map OccupancyGrid (indoor only) /rtabmap/odom Odometry (indoor only) /uwb/target PoseStamped (UWB position, base_link) /uwb/ranges UwbRangeArray (raw anchor ranges) /person/target PoseStamped (camera position, base_link) /person/detections Detection2DArray /cmd_vel Twist (from follower or Nav2) /saltybot/cmd String (to STM32) /saltybot/imu Imu /saltybot/balance_state String """ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, LogInfo, TimerAction, ) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression # ── Convenience: package share paths ────────────────────────────────────────── def _pkg(name: str) -> str: return get_package_share_directory(name) def _launch(pkg: str, *parts: str) -> PythonLaunchDescriptionSource: return PythonLaunchDescriptionSource(os.path.join(_pkg(pkg), *parts)) # ── Mode helpers ─────────────────────────────────────────────────────────────── def _mode_is(mode_str: str): """IfCondition: true when 'mode' arg == mode_str.""" return IfCondition( PythonExpression(["'", LaunchConfiguration("mode"), f"' == '{mode_str}'"]) ) def _mode_in(*modes): """IfCondition: true when 'mode' arg is one of the given strings.""" mode_set = repr(set(modes)) return IfCondition( PythonExpression(["'", LaunchConfiguration("mode"), f"' in {mode_set}"]) ) # ── Main ────────────────────────────────────────────────────────────────────── def generate_launch_description(): # ── Launch arguments ────────────────────────────────────────────────────── mode_arg = DeclareLaunchArgument( "mode", default_value="indoor", choices=["indoor", "outdoor", "follow"], description=( "Stack mode — indoor: SLAM+Nav2+follow; " "outdoor: GPS nav+follow; " "follow: sensors+UWB+perception+follower only" ), ) use_sim_time_arg = DeclareLaunchArgument( "use_sim_time", default_value="false", description="Use /clock from rosbag/simulator instead of wall clock", ) enable_csi_cameras_arg = DeclareLaunchArgument( "enable_csi_cameras", default_value="true", description="Launch 4× IMX219 CSI surround cameras", ) enable_uwb_arg = DeclareLaunchArgument( "enable_uwb", default_value="true", description="Launch UWB driver (requires MaUWB anchors on USB)", ) enable_perception_arg = DeclareLaunchArgument( "enable_perception", default_value="true", description="Launch YOLOv8n person detection (TensorRT)", ) enable_follower_arg = DeclareLaunchArgument( "enable_follower", default_value="true", description="Launch proportional person-follower controller", ) enable_bridge_arg = DeclareLaunchArgument( "enable_bridge", default_value="true", description="Launch STM32 serial bridge + cmd_vel bridge (disable for sim/rosbag)", ) enable_rosbridge_arg = DeclareLaunchArgument( "enable_rosbridge", default_value="true", description="Launch rosbridge WebSocket server (port 9090)", ) follow_distance_arg = DeclareLaunchArgument( "follow_distance", default_value="1.5", description="Person-follower target distance (m)", ) max_linear_vel_arg = DeclareLaunchArgument( "max_linear_vel", default_value="0.5", description="Max forward velocity cap (m/s) — forwarded to both follower and cmd_vel bridge", ) uwb_port_a_arg = DeclareLaunchArgument( "uwb_port_a", default_value="/dev/uwb-anchor0", description="UWB anchor-0 serial port (port/left side)", ) uwb_port_b_arg = DeclareLaunchArgument( "uwb_port_b", default_value="/dev/uwb-anchor1", description="UWB anchor-1 serial port (starboard/right side)", ) stm32_port_arg = DeclareLaunchArgument( "stm32_port", default_value="/dev/stm32-bridge", description="STM32 USB CDC serial port", ) # ── Shared substitution handles ─────────────────────────────────────────── mode = LaunchConfiguration("mode") use_sim_time = LaunchConfiguration("use_sim_time") follow_distance = LaunchConfiguration("follow_distance") max_linear_vel = LaunchConfiguration("max_linear_vel") uwb_port_a = LaunchConfiguration("uwb_port_a") uwb_port_b = LaunchConfiguration("uwb_port_b") stm32_port = LaunchConfiguration("stm32_port") # ── t=0s Robot description (URDF + TF tree) ────────────────────────────── robot_description = IncludeLaunchDescription( _launch("saltybot_description", "launch", "robot_description.launch.py"), launch_arguments={"use_sim_time": use_sim_time}.items(), ) # ── t=0s STM32 bidirectional serial bridge ──────────────────────────────── stm32_bridge = GroupAction( condition=IfCondition(LaunchConfiguration("enable_bridge")), actions=[ IncludeLaunchDescription( _launch("saltybot_bridge", "launch", "bridge.launch.py"), launch_arguments={ "mode": "bidirectional", "serial_port": stm32_port, }.items(), ), ], ) # ── t=2s cmd_vel safety bridge (depends on STM32 bridge) ──────────────── cmd_vel_bridge = TimerAction( period=2.0, actions=[ GroupAction( condition=IfCondition(LaunchConfiguration("enable_bridge")), actions=[ IncludeLaunchDescription( _launch("saltybot_bridge", "launch", "cmd_vel_bridge.launch.py"), launch_arguments={ "max_linear_vel": max_linear_vel, }.items(), ), ], ), ], ) # ── t=2s Sensors: RPLIDAR + RealSense D435i ────────────────────────────── sensors = TimerAction( period=2.0, actions=[ IncludeLaunchDescription( _launch("saltybot_bringup", "launch", "sensors.launch.py"), ), ], ) # ── t=3s LIDAR obstacle avoidance (360° safety with RPLIDAR A1M8) ──────── lidar_avoidance = TimerAction( period=3.0, actions=[ IncludeLaunchDescription( _launch("saltybot_lidar_avoidance", "launch", "lidar_avoidance.launch.py"), ), ], ) # ── t=4s Optional: 4× IMX219 CSI surround cameras ─────────────────────── csi_cameras = TimerAction( period=4.0, actions=[ GroupAction( condition=IfCondition(LaunchConfiguration("enable_csi_cameras")), actions=[ IncludeLaunchDescription( _launch("saltybot_cameras", "launch", "csi_cameras.launch.py"), ), ], ), ], ) # ── t=4s UWB driver (independent serial — can start early) ────────────── uwb_driver = TimerAction( period=4.0, actions=[ GroupAction( condition=IfCondition(LaunchConfiguration("enable_uwb")), actions=[ IncludeLaunchDescription( _launch("saltybot_uwb", "launch", "uwb.launch.py"), launch_arguments={ "port_a": uwb_port_a, "port_b": uwb_port_b, }.items(), ), ], ), ], ) # ── t=6s SLAM — RTAB-Map (indoor only; needs sensors up for ~4s) ───────── slam = TimerAction( period=6.0, actions=[ GroupAction( condition=_mode_is("indoor"), actions=[ LogInfo(msg="[full_stack] Starting RTAB-Map SLAM (indoor mode)"), IncludeLaunchDescription( _launch("saltybot_bringup", "launch", "slam_rtabmap.launch.py"), launch_arguments={ "use_sim_time": use_sim_time, }.items(), ), ], ), ], ) # ── t=6s Outdoor navigation (outdoor only; no SLAM) ───────────────────── outdoor_nav = TimerAction( period=6.0, actions=[ GroupAction( condition=_mode_is("outdoor"), actions=[ LogInfo(msg="[full_stack] Starting outdoor GPS navigation"), IncludeLaunchDescription( _launch("saltybot_outdoor", "launch", "outdoor_nav.launch.py"), launch_arguments={ "use_sim_time": use_sim_time, }.items(), ), ], ), ], ) # ── t=6s Person detection (needs RealSense up for ~4s) ────────────────── perception = TimerAction( period=6.0, actions=[ GroupAction( condition=IfCondition(LaunchConfiguration("enable_perception")), actions=[ LogInfo(msg="[full_stack] Starting YOLOv8n person detection"), IncludeLaunchDescription( _launch("saltybot_perception", "launch", "person_detection.launch.py"), launch_arguments={ "use_sim_time": use_sim_time, }.items(), ), ], ), ], ) # ── t=9s Person follower (needs perception + UWB; ~3s after both start) ─ follower = TimerAction( period=9.0, actions=[ GroupAction( condition=IfCondition(LaunchConfiguration("enable_follower")), actions=[ LogInfo(msg="[full_stack] Starting person follower"), IncludeLaunchDescription( _launch("saltybot_follower", "launch", "person_follower.launch.py"), launch_arguments={ "follow_distance": follow_distance, "max_linear_vel": max_linear_vel, }.items(), ), ], ), ], ) # ── t=14s Nav2 (indoor only; SLAM needs ~8s to build initial map) ──────── nav2 = TimerAction( period=14.0, actions=[ GroupAction( condition=_mode_is("indoor"), actions=[ LogInfo(msg="[full_stack] Starting Nav2 navigation stack"), IncludeLaunchDescription( _launch("saltybot_bringup", "launch", "nav2.launch.py"), ), ], ), ], ) # ── t=17s rosbridge WebSocket (last — all topics must be alive) ────────── rosbridge = TimerAction( period=17.0, actions=[ GroupAction( condition=IfCondition(LaunchConfiguration("enable_rosbridge")), actions=[ LogInfo(msg="[full_stack] Starting rosbridge (ws://0.0.0.0:9090)"), IncludeLaunchDescription( _launch("saltybot_bringup", "launch", "rosbridge.launch.py"), ), ], ), ], ) # ── Startup banner ──────────────────────────────────────────────────────── banner = LogInfo( msg=["[full_stack] SaltyBot bringup starting — mode=", mode, " sim_time=", use_sim_time] ) # ── Assemble ────────────────────────────────────────────────────────────── return LaunchDescription([ # Arguments mode_arg, use_sim_time_arg, enable_csi_cameras_arg, enable_uwb_arg, enable_perception_arg, enable_follower_arg, enable_bridge_arg, enable_rosbridge_arg, follow_distance_arg, max_linear_vel_arg, uwb_port_a_arg, uwb_port_b_arg, stm32_port_arg, # Startup banner banner, # t=0s robot_description, stm32_bridge, # t=2s sensors, cmd_vel_bridge, # t=3s lidar_avoidance, # t=4s csi_cameras, uwb_driver, # t=6s slam, outdoor_nav, perception, # t=9s follower, # t=14s nav2, # t=17s rosbridge, ])