Merge pull request 'feat: full_stack.launch.py — one-command autonomous stack bringup' (#68) from sl-jetson/full-stack-launch into main

This commit is contained in:
seb 2026-03-01 01:00:30 -05:00
commit b97ce09d80
2 changed files with 482 additions and 2 deletions

View File

@ -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,
])

View File

@ -3,8 +3,8 @@
<package format="3"> <package format="3">
<name>saltybot_bringup</name> <name>saltybot_bringup</name>
<version>0.1.0</version> <version>0.1.0</version>
<description>SaltyBot launch files — RealSense D435i + RPLIDAR A1M8 sensor bringup and SLAM integration</description> <description>SaltyBot launch files — full autonomous stack bringup (sensors, SLAM, Nav2, UWB, perception, follower)</description>
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer> <maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
<license>MIT</license> <license>MIT</license>
<exec_depend>rplidar_ros</exec_depend> <exec_depend>rplidar_ros</exec_depend>
@ -18,6 +18,14 @@
<exec_depend>rosbridge_server</exec_depend> <exec_depend>rosbridge_server</exec_depend>
<exec_depend>image_transport</exec_depend> <exec_depend>image_transport</exec_depend>
<exec_depend>image_transport_plugins</exec_depend> <exec_depend>image_transport_plugins</exec_depend>
<!-- SaltyBot packages included by full_stack.launch.py -->
<exec_depend>saltybot_bridge</exec_depend>
<exec_depend>saltybot_cameras</exec_depend>
<exec_depend>saltybot_description</exec_depend>
<exec_depend>saltybot_follower</exec_depend>
<exec_depend>saltybot_outdoor</exec_depend>
<exec_depend>saltybot_perception</exec_depend>
<exec_depend>saltybot_uwb</exec_depend>
<buildtool_depend>ament_python</buildtool_depend> <buildtool_depend>ament_python</buildtool_depend>