saltylab-firmware/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py

"""
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_object_detection_arg = DeclareLaunchArgument(
        "enable_object_detection",
        default_value="true",
        description="Launch YOLOv8n general object detection with depth (Issue #468)",
    )

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

    enable_docking_arg = DeclareLaunchArgument(
        "enable_docking",
        default_value="true",
        description="Launch autonomous docking behavior (auto-dock at 20% battery, Issue #489)",
    )

    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=6s  Object detection (needs RealSense up for ~4s; Issue #468) ──────
    object_detection = TimerAction(
        period=6.0,
        actions=[
            GroupAction(
                condition=IfCondition(LaunchConfiguration("enable_object_detection")),
                actions=[
                    LogInfo(msg="[full_stack] Starting YOLOv8n general object detection"),
                    IncludeLaunchDescription(
                        _launch("saltybot_object_detection", "launch", "object_detection.launch.py"),
                    ),
                ],
            ),
        ],
    )

    # ── 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=7s  Docking (auto-dock at 20% battery, Issue #489) ──────────────────
    docking = TimerAction(
        period=7.0,
        actions=[
            GroupAction(
                condition=IfCondition(LaunchConfiguration("enable_docking")),
                actions=[
                    LogInfo(msg="[full_stack] Starting autonomous docking (auto-trigger at 20% battery)"),
                    IncludeLaunchDescription(
                        _launch("saltybot_docking", "launch", "docking.launch.py"),
                        launch_arguments={
                            "battery_low_pct": "20.0",
                        }.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_object_detection_arg,
        enable_follower_arg,
        enable_bridge_arg,
        enable_rosbridge_arg,
        enable_docking_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,
        object_detection,

        # t=7s
        docking,

        # t=9s
        follower,

        # t=14s
        nav2,

        # t=17s
        rosbridge,
    ])