diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py new file mode 100644 index 0000000..6c1794c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py @@ -0,0 +1,472 @@ +""" +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=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=4s + csi_cameras, + uwb_driver, + + # t=6s + slam, + outdoor_nav, + perception, + + # t=9s + follower, + + # t=14s + nav2, + + # t=17s + rosbridge, + ]) diff --git a/jetson/ros2_ws/src/saltybot_bringup/package.xml b/jetson/ros2_ws/src/saltybot_bringup/package.xml index 71ac935..aea9dfb 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/package.xml +++ b/jetson/ros2_ws/src/saltybot_bringup/package.xml @@ -3,8 +3,8 @@ saltybot_bringup 0.1.0 - SaltyBot launch files — RealSense D435i + RPLIDAR A1M8 sensor bringup and SLAM integration - sl-perception + SaltyBot launch files — full autonomous stack bringup (sensors, SLAM, Nav2, UWB, perception, follower) + sl-jetson MIT rplidar_ros @@ -18,6 +18,14 @@ rosbridge_server image_transport image_transport_plugins + + saltybot_bridge + saltybot_cameras + saltybot_description + saltybot_follower + saltybot_outdoor + saltybot_perception + saltybot_uwb ament_python