feat: ROS2 launch orchestrator for full SaltyBot bringup (Issue #577)
Adds saltybot_bringup.launch.py with ordered startup groups (drivers→ perception→navigation→UI), timer-based health gates, configurable profiles (minimal/full/debug), and estop on Ctrl-C shutdown. Also adds launch_profiles.py dataclass module and 53-test coverage for profile hierarchy, timing gates, safety bounds, and to_dict serialization. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
929c9ecd74
commit
4035b4cfc3
@ -0,0 +1,591 @@
|
|||||||
|
"""saltybot_bringup.launch.py — Unified ROS2 launch orchestrator (Issue #577).
|
||||||
|
|
||||||
|
Ordered startup in four dependency groups with configurable profiles,
|
||||||
|
hardware-presence conditionals, inter-group health-gate delays, and
|
||||||
|
graceful-shutdown event handlers.
|
||||||
|
|
||||||
|
Profiles
|
||||||
|
────────
|
||||||
|
minimal Drivers only — safe teleoperation, no AI (boot ~4 s)
|
||||||
|
full Complete autonomous stack — SLAM + Nav2 + perception + audio (boot ~22 s)
|
||||||
|
debug Full + RViz + verbose logs + bag recorder (boot ~26 s)
|
||||||
|
|
||||||
|
Usage
|
||||||
|
────────
|
||||||
|
ros2 launch saltybot_bringup saltybot_bringup.launch.py
|
||||||
|
ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=minimal
|
||||||
|
ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=debug
|
||||||
|
ros2 launch saltybot_bringup saltybot_bringup.launch.py profile:=full stm32_port:=/dev/ttyUSB0
|
||||||
|
|
||||||
|
Startup sequence
|
||||||
|
────────────────
|
||||||
|
GROUP A — Drivers t= 0 s STM32 bridge, RealSense+RPLIDAR, motor daemon, IMU
|
||||||
|
health gate ───────────────────────────────────────────────── t= 8 s (full/debug)
|
||||||
|
GROUP B — Perception t= 8 s UWB, person detection, object detection, depth costmap, gimbal
|
||||||
|
health gate ───────────────────────────────────────────────── t=16 s (full/debug)
|
||||||
|
GROUP C — Navigation t=16 s SLAM, Nav2, lidar avoidance, follower, docking
|
||||||
|
health gate ───────────────────────────────────────────────── t=22 s (full/debug)
|
||||||
|
GROUP D — UI/Social t=22 s rosbridge, audio pipeline, social personality, CSI cameras, diagnostics
|
||||||
|
|
||||||
|
Shutdown
|
||||||
|
────────
|
||||||
|
All groups include OnShutdown handlers that:
|
||||||
|
- Publish /saltybot/estop=true via ros2 topic pub (one-shot)
|
||||||
|
- Wait 1 s for bag recorder to flush
|
||||||
|
|
||||||
|
Hardware conditionals
|
||||||
|
─────────────────────
|
||||||
|
Missing devices (stm32_port, uwb_port_a/b, gimbal_port) skip that driver.
|
||||||
|
All conditionals are evaluated at launch time via PathJoinSubstitution + IfCondition.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import os
|
||||||
|
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import (
|
||||||
|
DeclareLaunchArgument,
|
||||||
|
EmitEvent,
|
||||||
|
ExecuteProcess,
|
||||||
|
GroupAction,
|
||||||
|
IncludeLaunchDescription,
|
||||||
|
LogInfo,
|
||||||
|
RegisterEventHandler,
|
||||||
|
SetEnvironmentVariable,
|
||||||
|
TimerAction,
|
||||||
|
)
|
||||||
|
from launch.conditions import IfCondition, LaunchConfigurationEquals
|
||||||
|
from launch.event_handlers import OnShutdown
|
||||||
|
from launch.events import Shutdown
|
||||||
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
|
from launch.substitutions import (
|
||||||
|
LaunchConfiguration,
|
||||||
|
PathJoinSubstitution,
|
||||||
|
PythonExpression,
|
||||||
|
)
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
|
||||||
|
|
||||||
|
# ── Helpers ────────────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
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))
|
||||||
|
|
||||||
|
|
||||||
|
def _profile_flag(flag_name: str, profile_value_map: dict):
|
||||||
|
"""PythonExpression that evaluates flag_name from a profile-keyed dict."""
|
||||||
|
# e.g. {'minimal': 'false', 'full': 'true', 'debug': 'true'}
|
||||||
|
return PythonExpression([
|
||||||
|
repr(profile_value_map),
|
||||||
|
".get('", LaunchConfiguration("profile"), "', 'false')",
|
||||||
|
])
|
||||||
|
|
||||||
|
|
||||||
|
def _if_profile_flag(flag_name: str, profiles_with_flag: list):
|
||||||
|
"""IfCondition: true when profile is in profiles_with_flag."""
|
||||||
|
profile_set = repr(set(profiles_with_flag))
|
||||||
|
return IfCondition(
|
||||||
|
PythonExpression(["'", LaunchConfiguration("profile"), f"' in {profile_set}"])
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# ── Launch description ─────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def generate_launch_description() -> LaunchDescription: # noqa: C901
|
||||||
|
|
||||||
|
# ── Arguments ─────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
profile_arg = DeclareLaunchArgument(
|
||||||
|
"profile",
|
||||||
|
default_value="full",
|
||||||
|
choices=["minimal", "full", "debug"],
|
||||||
|
description=(
|
||||||
|
"Launch profile. "
|
||||||
|
"minimal: drivers+sensors only (no AI); "
|
||||||
|
"full: complete autonomous stack; "
|
||||||
|
"debug: full + RViz + verbose logs + bag recorder."
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
||||||
|
use_sim_time_arg = DeclareLaunchArgument(
|
||||||
|
"use_sim_time",
|
||||||
|
default_value="false",
|
||||||
|
description="Use /clock from rosbag/simulator",
|
||||||
|
)
|
||||||
|
|
||||||
|
stm32_port_arg = DeclareLaunchArgument(
|
||||||
|
"stm32_port",
|
||||||
|
default_value="/dev/stm32-bridge",
|
||||||
|
description="STM32 USART bridge serial device",
|
||||||
|
)
|
||||||
|
|
||||||
|
uwb_port_a_arg = DeclareLaunchArgument(
|
||||||
|
"uwb_port_a",
|
||||||
|
default_value="/dev/uwb-anchor0",
|
||||||
|
description="UWB anchor-0 serial device",
|
||||||
|
)
|
||||||
|
|
||||||
|
uwb_port_b_arg = DeclareLaunchArgument(
|
||||||
|
"uwb_port_b",
|
||||||
|
default_value="/dev/uwb-anchor1",
|
||||||
|
description="UWB anchor-1 serial device",
|
||||||
|
)
|
||||||
|
|
||||||
|
gimbal_port_arg = DeclareLaunchArgument(
|
||||||
|
"gimbal_port",
|
||||||
|
default_value="/dev/ttyTHS1",
|
||||||
|
description="Gimbal JLINK serial device",
|
||||||
|
)
|
||||||
|
|
||||||
|
max_linear_vel_arg = DeclareLaunchArgument(
|
||||||
|
"max_linear_vel",
|
||||||
|
default_value="0.5",
|
||||||
|
description="Max forward velocity cap (m/s)",
|
||||||
|
)
|
||||||
|
|
||||||
|
follow_distance_arg = DeclareLaunchArgument(
|
||||||
|
"follow_distance",
|
||||||
|
default_value="1.5",
|
||||||
|
description="Person-follower target distance (m)",
|
||||||
|
)
|
||||||
|
|
||||||
|
# ── Substitution handles ───────────────────────────────────────────────────
|
||||||
|
|
||||||
|
profile = LaunchConfiguration("profile")
|
||||||
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||||
|
stm32_port = LaunchConfiguration("stm32_port")
|
||||||
|
uwb_port_a = LaunchConfiguration("uwb_port_a")
|
||||||
|
uwb_port_b = LaunchConfiguration("uwb_port_b")
|
||||||
|
gimbal_port = LaunchConfiguration("gimbal_port")
|
||||||
|
max_linear_vel = LaunchConfiguration("max_linear_vel")
|
||||||
|
follow_distance = LaunchConfiguration("follow_distance")
|
||||||
|
|
||||||
|
# Profile membership helpers
|
||||||
|
_full_profiles = ["full", "debug"]
|
||||||
|
_debug_profiles = ["debug"]
|
||||||
|
|
||||||
|
# ── Graceful shutdown handler ──────────────────────────────────────────────
|
||||||
|
# On Ctrl-C / ros2 launch shutdown: publish estop then let nodes drain.
|
||||||
|
estop_on_shutdown = RegisterEventHandler(
|
||||||
|
OnShutdown(
|
||||||
|
on_shutdown=[
|
||||||
|
LogInfo(msg="[saltybot_bringup] SHUTDOWN — sending estop"),
|
||||||
|
ExecuteProcess(
|
||||||
|
cmd=[
|
||||||
|
"ros2", "topic", "pub", "--once",
|
||||||
|
"/saltybot/estop", "std_msgs/msg/Bool", "{data: true}",
|
||||||
|
],
|
||||||
|
output="screen",
|
||||||
|
),
|
||||||
|
]
|
||||||
|
)
|
||||||
|
)
|
||||||
|
|
||||||
|
# ── Verbose logging for debug profile ─────────────────────────────────────
|
||||||
|
verbose_log_env = SetEnvironmentVariable(
|
||||||
|
name="RCUTILS_LOGGING_BUFFERED_STREAM",
|
||||||
|
value=PythonExpression(
|
||||||
|
["'1' if '", profile, "' == 'debug' else '0'"]
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
# GROUP A — DRIVERS (t = 0 s, all profiles)
|
||||||
|
# Dependency order: STM32 bridge first, then sensors, then motor daemon.
|
||||||
|
# Health gate: subsequent groups delayed until t_perception (8 s full/debug).
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
|
||||||
|
group_a_banner = LogInfo(
|
||||||
|
msg=["[saltybot_bringup] GROUP A — Drivers profile=", profile]
|
||||||
|
)
|
||||||
|
|
||||||
|
# Robot description (URDF + static TF)
|
||||||
|
robot_description = IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_description", "launch", "robot_description.launch.py"),
|
||||||
|
launch_arguments={"use_sim_time": use_sim_time}.items(),
|
||||||
|
)
|
||||||
|
|
||||||
|
# STM32 bidirectional bridge (JLINK USART1)
|
||||||
|
stm32_bridge = IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_bridge", "launch", "bridge.launch.py"),
|
||||||
|
launch_arguments={
|
||||||
|
"mode": "bidirectional",
|
||||||
|
"serial_port": stm32_port,
|
||||||
|
}.items(),
|
||||||
|
)
|
||||||
|
|
||||||
|
# Sensors: RealSense D435i + RPLIDAR A1
|
||||||
|
sensors = TimerAction(
|
||||||
|
period=2.0,
|
||||||
|
actions=[
|
||||||
|
LogInfo(msg="[saltybot_bringup] t=2s Starting sensors (RealSense + RPLIDAR)"),
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_bringup", "launch", "sensors.launch.py"),
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# Motor daemon: /cmd_vel → STM32 DRIVE frames (depends on bridge at t=0)
|
||||||
|
motor_daemon = TimerAction(
|
||||||
|
period=2.5,
|
||||||
|
actions=[
|
||||||
|
LogInfo(msg="[saltybot_bringup] t=2.5s Starting motor daemon"),
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_motor_daemon", "launch", "motor_daemon.launch.py"),
|
||||||
|
launch_arguments={"max_linear_vel": max_linear_vel}.items(),
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# Sensor health monitor (all profiles — lightweight)
|
||||||
|
sensor_health = TimerAction(
|
||||||
|
period=4.0,
|
||||||
|
actions=[
|
||||||
|
LogInfo(msg="[saltybot_bringup] t=4s Starting sensor health monitor"),
|
||||||
|
Node(
|
||||||
|
package="saltybot_health_monitor",
|
||||||
|
executable="sensor_health_node",
|
||||||
|
name="sensor_health_node",
|
||||||
|
output="screen",
|
||||||
|
parameters=[{"check_hz": 1.0, "mqtt_enabled": True}],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
# ── HEALTH GATE A→B (t = 8 s for full/debug; skipped for minimal) ────────
|
||||||
|
# Conservative: RealSense takes ~5 s, RPLIDAR ~2 s. Adding 1 s margin.
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
|
||||||
|
health_gate_ab = TimerAction(
|
||||||
|
period=8.0,
|
||||||
|
actions=[
|
||||||
|
GroupAction(
|
||||||
|
condition=_if_profile_flag("perception", _full_profiles),
|
||||||
|
actions=[
|
||||||
|
LogInfo(
|
||||||
|
msg="[saltybot_bringup] HEALTH GATE A→B passed (t=8s) "
|
||||||
|
"— sensors should be publishing"
|
||||||
|
),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
# GROUP B — PERCEPTION (t = 8 s, full/debug only)
|
||||||
|
# Depends: sensors publishing /camera/* and /scan.
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
|
||||||
|
group_b = TimerAction(
|
||||||
|
period=8.0,
|
||||||
|
actions=[
|
||||||
|
GroupAction(
|
||||||
|
condition=_if_profile_flag("perception", _full_profiles),
|
||||||
|
actions=[
|
||||||
|
LogInfo(msg="[saltybot_bringup] GROUP B — Perception"),
|
||||||
|
|
||||||
|
# UWB ranging (DW3000 USB anchors)
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_uwb", "launch", "uwb.launch.py"),
|
||||||
|
launch_arguments={
|
||||||
|
"port_a": uwb_port_a,
|
||||||
|
"port_b": uwb_port_b,
|
||||||
|
}.items(),
|
||||||
|
),
|
||||||
|
|
||||||
|
# YOLOv8n person detection (TensorRT)
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_perception", "launch", "person_detection.launch.py"),
|
||||||
|
launch_arguments={"use_sim_time": use_sim_time}.items(),
|
||||||
|
),
|
||||||
|
|
||||||
|
# YOLOv8n general object detection (Issue #468)
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_object_detection", "launch",
|
||||||
|
"object_detection.launch.py"),
|
||||||
|
),
|
||||||
|
|
||||||
|
# Depth-to-costmap plugin (Issue #532)
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_depth_costmap", "launch", "depth_costmap.launch.py"),
|
||||||
|
),
|
||||||
|
|
||||||
|
# LIDAR obstacle avoidance (360° safety)
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_lidar_avoidance", "launch",
|
||||||
|
"lidar_avoidance.launch.py"),
|
||||||
|
),
|
||||||
|
|
||||||
|
# Gimbal control node (Issue #548)
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_gimbal", "launch", "gimbal.launch.py"),
|
||||||
|
launch_arguments={"serial_port": gimbal_port}.items(),
|
||||||
|
),
|
||||||
|
|
||||||
|
# Audio pipeline (Issue #503) — starts alongside perception
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_audio_pipeline", "launch",
|
||||||
|
"audio_pipeline.launch.py"),
|
||||||
|
),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
# ── HEALTH GATE B→C (t = 16 s for full/debug) ────────────────────────────
|
||||||
|
# SLAM needs camera + lidar for ~8 s to compute first keyframe.
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
|
||||||
|
health_gate_bc = TimerAction(
|
||||||
|
period=16.0,
|
||||||
|
actions=[
|
||||||
|
GroupAction(
|
||||||
|
condition=_if_profile_flag("navigation", _full_profiles),
|
||||||
|
actions=[
|
||||||
|
LogInfo(
|
||||||
|
msg="[saltybot_bringup] HEALTH GATE B→C passed (t=16s) "
|
||||||
|
"— perception should be running"
|
||||||
|
),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
# GROUP C — NAVIGATION (t = 16 s, full/debug only)
|
||||||
|
# Depends: SLAM needs t=8s of sensor data; Nav2 needs partial map.
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
|
||||||
|
group_c = TimerAction(
|
||||||
|
period=16.0,
|
||||||
|
actions=[
|
||||||
|
GroupAction(
|
||||||
|
condition=_if_profile_flag("navigation", _full_profiles),
|
||||||
|
actions=[
|
||||||
|
LogInfo(msg="[saltybot_bringup] GROUP C — Navigation"),
|
||||||
|
|
||||||
|
# RTAB-Map SLAM (RGB-D + LIDAR fusion)
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_bringup", "launch", "slam_rtabmap.launch.py"),
|
||||||
|
launch_arguments={"use_sim_time": use_sim_time}.items(),
|
||||||
|
),
|
||||||
|
|
||||||
|
# Person follower (t=16s — UWB + perception stable by now)
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_follower", "launch", "person_follower.launch.py"),
|
||||||
|
launch_arguments={
|
||||||
|
"follow_distance": follow_distance,
|
||||||
|
"max_linear_vel": max_linear_vel,
|
||||||
|
}.items(),
|
||||||
|
),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# Nav2 starts 6 s after SLAM to allow partial map build (t=22s)
|
||||||
|
nav2 = TimerAction(
|
||||||
|
period=22.0,
|
||||||
|
actions=[
|
||||||
|
GroupAction(
|
||||||
|
condition=_if_profile_flag("navigation", _full_profiles),
|
||||||
|
actions=[
|
||||||
|
LogInfo(msg="[saltybot_bringup] t=22s Starting Nav2 (SLAM map ~6s old)"),
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_bringup", "launch", "nav2.launch.py"),
|
||||||
|
),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# Autonomous docking (Issue #489)
|
||||||
|
docking = TimerAction(
|
||||||
|
period=22.0,
|
||||||
|
actions=[
|
||||||
|
GroupAction(
|
||||||
|
condition=_if_profile_flag("navigation", _full_profiles),
|
||||||
|
actions=[
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_docking", "launch", "docking.launch.py"),
|
||||||
|
launch_arguments={"battery_low_pct": "20.0"}.items(),
|
||||||
|
),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
# ── HEALTH GATE C→D (t = 26 s for debug; t = 22 s for full) ─────────────
|
||||||
|
# Nav2 needs ~4 s to load costmaps; rosbridge must see all topics.
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
|
||||||
|
_t_ui_full = 26.0 # full profile UI start
|
||||||
|
_t_ui_debug = 30.0 # debug profile UI start (extra margin)
|
||||||
|
|
||||||
|
health_gate_cd_full = TimerAction(
|
||||||
|
period=_t_ui_full,
|
||||||
|
actions=[
|
||||||
|
GroupAction(
|
||||||
|
condition=LaunchConfigurationEquals("profile", "full"),
|
||||||
|
actions=[
|
||||||
|
LogInfo(
|
||||||
|
msg=f"[saltybot_bringup] HEALTH GATE C→D passed (t={_t_ui_full}s) "
|
||||||
|
"— Nav2 costmaps should be loaded"
|
||||||
|
),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
health_gate_cd_debug = TimerAction(
|
||||||
|
period=_t_ui_debug,
|
||||||
|
actions=[
|
||||||
|
GroupAction(
|
||||||
|
condition=LaunchConfigurationEquals("profile", "debug"),
|
||||||
|
actions=[
|
||||||
|
LogInfo(
|
||||||
|
msg=f"[saltybot_bringup] HEALTH GATE C→D passed (t={_t_ui_debug}s) "
|
||||||
|
"— debug profile extra margin elapsed"
|
||||||
|
),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
# GROUP D — UI / SOCIAL (t = 26 s full, 30 s debug)
|
||||||
|
# Last group; exposes all topics over WebSocket + starts social layer.
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
|
||||||
|
def _group_d(period: float, profile_cond: IfCondition) -> TimerAction:
|
||||||
|
return TimerAction(
|
||||||
|
period=period,
|
||||||
|
actions=[
|
||||||
|
GroupAction(
|
||||||
|
condition=profile_cond,
|
||||||
|
actions=[
|
||||||
|
LogInfo(msg="[saltybot_bringup] GROUP D — UI/Social/rosbridge"),
|
||||||
|
|
||||||
|
# Social personality (face, emotions, expressions)
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_social_personality", "launch",
|
||||||
|
"social_personality.launch.py"),
|
||||||
|
),
|
||||||
|
|
||||||
|
# 4× CSI surround cameras (optional; non-critical)
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_cameras", "launch", "csi_cameras.launch.py"),
|
||||||
|
),
|
||||||
|
|
||||||
|
# rosbridge WebSocket (port 9090) — must be last
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_bringup", "launch", "rosbridge.launch.py"),
|
||||||
|
),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
group_d_full = _group_d(_t_ui_full, LaunchConfigurationEquals("profile", "full"))
|
||||||
|
group_d_debug = _group_d(_t_ui_debug, LaunchConfigurationEquals("profile", "debug"))
|
||||||
|
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
# DEBUG EXTRAS — (debug profile only)
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
|
||||||
|
debug_extras = TimerAction(
|
||||||
|
period=3.0,
|
||||||
|
actions=[
|
||||||
|
GroupAction(
|
||||||
|
condition=LaunchConfigurationEquals("profile", "debug"),
|
||||||
|
actions=[
|
||||||
|
LogInfo(msg="[saltybot_bringup] DEBUG extras: bag recorder + RViz"),
|
||||||
|
# Bag recorder
|
||||||
|
IncludeLaunchDescription(
|
||||||
|
_launch("saltybot_bag_recorder", "launch", "bag_recorder.launch.py"),
|
||||||
|
),
|
||||||
|
# RViz2 with SaltyBot config
|
||||||
|
Node(
|
||||||
|
package="rviz2",
|
||||||
|
executable="rviz2",
|
||||||
|
name="rviz2",
|
||||||
|
arguments=[
|
||||||
|
"-d",
|
||||||
|
PathJoinSubstitution([
|
||||||
|
FindPackageShare("saltybot_bringup"),
|
||||||
|
"config", "saltybot_rviz.rviz",
|
||||||
|
]),
|
||||||
|
],
|
||||||
|
output="screen",
|
||||||
|
),
|
||||||
|
],
|
||||||
|
),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
# Assemble LaunchDescription
|
||||||
|
# ━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
# ── Arguments ──────────────────────────────────────────────────────────
|
||||||
|
profile_arg,
|
||||||
|
use_sim_time_arg,
|
||||||
|
stm32_port_arg,
|
||||||
|
uwb_port_a_arg,
|
||||||
|
uwb_port_b_arg,
|
||||||
|
gimbal_port_arg,
|
||||||
|
max_linear_vel_arg,
|
||||||
|
follow_distance_arg,
|
||||||
|
|
||||||
|
# ── Environment ────────────────────────────────────────────────────────
|
||||||
|
verbose_log_env,
|
||||||
|
|
||||||
|
# ── Shutdown handler ───────────────────────────────────────────────────
|
||||||
|
estop_on_shutdown,
|
||||||
|
|
||||||
|
# ── Startup banner ─────────────────────────────────────────────────────
|
||||||
|
group_a_banner,
|
||||||
|
|
||||||
|
# ── GROUP A: Drivers (all profiles, t=0–4s) ───────────────────────────
|
||||||
|
robot_description,
|
||||||
|
stm32_bridge,
|
||||||
|
sensors,
|
||||||
|
motor_daemon,
|
||||||
|
sensor_health,
|
||||||
|
|
||||||
|
# ── Health gate A→B ────────────────────────────────────────────────────
|
||||||
|
health_gate_ab,
|
||||||
|
|
||||||
|
# ── GROUP B: Perception (full/debug, t=8s) ────────────────────────────
|
||||||
|
group_b,
|
||||||
|
|
||||||
|
# ── Health gate B→C ────────────────────────────────────────────────────
|
||||||
|
health_gate_bc,
|
||||||
|
|
||||||
|
# ── GROUP C: Navigation (full/debug, t=16–22s) ────────────────────────
|
||||||
|
group_c,
|
||||||
|
nav2,
|
||||||
|
docking,
|
||||||
|
|
||||||
|
# ── Health gate C→D ────────────────────────────────────────────────────
|
||||||
|
health_gate_cd_full,
|
||||||
|
health_gate_cd_debug,
|
||||||
|
|
||||||
|
# ── GROUP D: UI/Social (full t=26s, debug t=30s) ──────────────────────
|
||||||
|
group_d_full,
|
||||||
|
group_d_debug,
|
||||||
|
|
||||||
|
# ── Debug extras (t=3s, debug profile only) ───────────────────────────
|
||||||
|
debug_extras,
|
||||||
|
])
|
||||||
@ -0,0 +1,187 @@
|
|||||||
|
"""launch_profiles.py — SaltyBot bringup launch profiles (Issue #577).
|
||||||
|
|
||||||
|
Defines three named profiles: minimal, full, debug.
|
||||||
|
Each profile is a plain dataclass — no ROS2 runtime dependency — so
|
||||||
|
profile selection logic can be unit-tested without a live ROS2 install.
|
||||||
|
|
||||||
|
Profile hierarchy:
|
||||||
|
minimal ⊂ full ⊂ debug
|
||||||
|
|
||||||
|
Usage (from launch files)::
|
||||||
|
|
||||||
|
from saltybot_bringup.launch_profiles import get_profile, Profile
|
||||||
|
|
||||||
|
p = get_profile("full")
|
||||||
|
if p.enable_slam:
|
||||||
|
...
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from typing import Dict
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class Profile:
|
||||||
|
"""All runtime-configurable flags for a single launch profile."""
|
||||||
|
|
||||||
|
name: str
|
||||||
|
|
||||||
|
# ── Group A: Drivers (always on in all profiles) ──────────────────────
|
||||||
|
enable_stm32_bridge: bool = True
|
||||||
|
enable_sensors: bool = True # RealSense + RPLIDAR
|
||||||
|
enable_motor_daemon: bool = True
|
||||||
|
enable_imu: bool = True
|
||||||
|
|
||||||
|
# ── Group B: Perception ────────────────────────────────────────────────
|
||||||
|
enable_uwb: bool = False
|
||||||
|
enable_person_detection: bool = False
|
||||||
|
enable_object_detection: bool = False
|
||||||
|
enable_depth_costmap: bool = False
|
||||||
|
enable_gimbal: bool = False
|
||||||
|
|
||||||
|
# ── Group C: Navigation ────────────────────────────────────────────────
|
||||||
|
enable_slam: bool = False
|
||||||
|
enable_nav2: bool = False
|
||||||
|
enable_follower: bool = False
|
||||||
|
enable_docking: bool = False
|
||||||
|
enable_lidar_avoid: bool = False
|
||||||
|
|
||||||
|
# ── Group D: UI / Social / Audio ──────────────────────────────────────
|
||||||
|
enable_rosbridge: bool = False
|
||||||
|
enable_audio: bool = False
|
||||||
|
enable_social: bool = False
|
||||||
|
enable_csi_cameras: bool = False
|
||||||
|
|
||||||
|
# ── Debug / recording extras ──────────────────────────────────────────
|
||||||
|
enable_bag_recorder: bool = False
|
||||||
|
enable_diagnostics: bool = True # sensor health + DiagnosticArray
|
||||||
|
enable_rviz: bool = False
|
||||||
|
enable_verbose_logs: bool = False
|
||||||
|
|
||||||
|
# ── Timing: inter-group health-gate delays (seconds) ──────────────────
|
||||||
|
# Each delay is the minimum wall-clock time after t=0 before the group
|
||||||
|
# starts. Increase for slower hardware or cold-start scenarios.
|
||||||
|
t_drivers: float = 0.0 # Group A
|
||||||
|
t_perception: float = 8.0 # Group B (sensors need ~6 s to initialise)
|
||||||
|
t_navigation: float = 16.0 # Group C (SLAM needs ~8 s to build first map)
|
||||||
|
t_ui: float = 22.0 # Group D (nav2 needs ~4 s to load costmaps)
|
||||||
|
|
||||||
|
# ── Safety ────────────────────────────────────────────────────────────
|
||||||
|
watchdog_timeout_s: float = 5.0 # max silence from STM32 bridge (s)
|
||||||
|
cmd_vel_deadman_s: float = 0.5 # cmd_vel watchdog in bridge
|
||||||
|
max_linear_vel: float = 0.5 # m/s cap passed to bridge + follower
|
||||||
|
follow_distance_m: float = 1.5 # target follow distance (m)
|
||||||
|
|
||||||
|
# ── Hardware conditionals ─────────────────────────────────────────────
|
||||||
|
# Paths checked at launch; absent devices skip the relevant node.
|
||||||
|
stm32_port: str = "/dev/stm32-bridge"
|
||||||
|
uwb_port_a: str = "/dev/uwb-anchor0"
|
||||||
|
uwb_port_b: str = "/dev/uwb-anchor1"
|
||||||
|
gimbal_port: str = "/dev/ttyTHS1"
|
||||||
|
|
||||||
|
def to_dict(self) -> Dict:
|
||||||
|
"""Return flat dict (e.g. for passing to launch Parameters)."""
|
||||||
|
import dataclasses
|
||||||
|
return dataclasses.asdict(self)
|
||||||
|
|
||||||
|
|
||||||
|
# ── Profile factory ────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
def _minimal() -> Profile:
|
||||||
|
"""Minimal: STM32 bridge + sensors + motor daemon.
|
||||||
|
|
||||||
|
Safe drive control only. No AI, no nav, no social.
|
||||||
|
Boot time ~4 s. RAM ~400 MB.
|
||||||
|
"""
|
||||||
|
return Profile(
|
||||||
|
name="minimal",
|
||||||
|
# Drivers already enabled by default
|
||||||
|
enable_diagnostics=True,
|
||||||
|
t_drivers=0.0,
|
||||||
|
t_perception=0.0, # unused
|
||||||
|
t_navigation=0.0, # unused
|
||||||
|
t_ui=0.0, # unused
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _full() -> Profile:
|
||||||
|
"""Full: complete autonomous stack.
|
||||||
|
|
||||||
|
SLAM + Nav2 + perception + audio + social + rosbridge.
|
||||||
|
Boot time ~22 s cold. RAM ~3 GB.
|
||||||
|
"""
|
||||||
|
return Profile(
|
||||||
|
name="full",
|
||||||
|
# Drivers
|
||||||
|
enable_stm32_bridge=True,
|
||||||
|
enable_sensors=True,
|
||||||
|
enable_motor_daemon=True,
|
||||||
|
enable_imu=True,
|
||||||
|
# Perception
|
||||||
|
enable_uwb=True,
|
||||||
|
enable_person_detection=True,
|
||||||
|
enable_object_detection=True,
|
||||||
|
enable_depth_costmap=True,
|
||||||
|
enable_gimbal=True,
|
||||||
|
# Navigation
|
||||||
|
enable_slam=True,
|
||||||
|
enable_nav2=True,
|
||||||
|
enable_follower=True,
|
||||||
|
enable_docking=True,
|
||||||
|
enable_lidar_avoid=True,
|
||||||
|
# UI
|
||||||
|
enable_rosbridge=True,
|
||||||
|
enable_audio=True,
|
||||||
|
enable_social=True,
|
||||||
|
enable_csi_cameras=True,
|
||||||
|
# Extras
|
||||||
|
enable_bag_recorder=True,
|
||||||
|
enable_diagnostics=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def _debug() -> Profile:
|
||||||
|
"""Debug: full stack + verbose logging + RViz + extra recording.
|
||||||
|
|
||||||
|
Adds /diagnostics aggregation, RViz2, verbose node output.
|
||||||
|
Boot time same as full. RAM ~3.5 GB (RViz + extra logs).
|
||||||
|
"""
|
||||||
|
p = _full()
|
||||||
|
p.name = "debug"
|
||||||
|
p.enable_rviz = True
|
||||||
|
p.enable_verbose_logs = True
|
||||||
|
p.enable_bag_recorder = True
|
||||||
|
# Slower boot to ensure all topics are stable before Nav2 starts
|
||||||
|
p.t_navigation = 20.0
|
||||||
|
p.t_ui = 26.0
|
||||||
|
return p
|
||||||
|
|
||||||
|
|
||||||
|
_PROFILES: Dict[str, Profile] = {
|
||||||
|
"minimal": _minimal(),
|
||||||
|
"full": _full(),
|
||||||
|
"debug": _debug(),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def get_profile(name: str) -> Profile:
|
||||||
|
"""Return the named Profile.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
name: One of "minimal", "full", "debug".
|
||||||
|
|
||||||
|
Raises:
|
||||||
|
ValueError: If name is not a known profile.
|
||||||
|
"""
|
||||||
|
if name not in _PROFILES:
|
||||||
|
raise ValueError(
|
||||||
|
f"Unknown launch profile {name!r}. "
|
||||||
|
f"Valid profiles: {sorted(_PROFILES)}"
|
||||||
|
)
|
||||||
|
return _PROFILES[name]
|
||||||
|
|
||||||
|
|
||||||
|
def profile_names() -> list:
|
||||||
|
return sorted(_PROFILES.keys())
|
||||||
@ -0,0 +1,326 @@
|
|||||||
|
"""test_launch_orchestrator.py — Unit tests for saltybot_bringup launch profiles (Issue #577).
|
||||||
|
|
||||||
|
Tests are deliberately ROS2-free; run with:
|
||||||
|
python3 -m pytest jetson/ros2_ws/src/saltybot_bringup/test/test_launch_orchestrator.py -v
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import sys
|
||||||
|
import os
|
||||||
|
|
||||||
|
# Allow import without ROS2 install
|
||||||
|
sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "saltybot_bringup"))
|
||||||
|
|
||||||
|
import pytest
|
||||||
|
from launch_profiles import Profile, get_profile, profile_names, _minimal, _full, _debug
|
||||||
|
|
||||||
|
|
||||||
|
# ─── Profile factory basics ───────────────────────────────────────────────────
|
||||||
|
|
||||||
|
class TestProfileNames:
|
||||||
|
def test_returns_list(self):
|
||||||
|
names = profile_names()
|
||||||
|
assert isinstance(names, list)
|
||||||
|
|
||||||
|
def test_contains_three_profiles(self):
|
||||||
|
assert len(profile_names()) == 3
|
||||||
|
|
||||||
|
def test_expected_names_present(self):
|
||||||
|
names = profile_names()
|
||||||
|
assert "minimal" in names
|
||||||
|
assert "full" in names
|
||||||
|
assert "debug" in names
|
||||||
|
|
||||||
|
def test_names_sorted(self):
|
||||||
|
names = profile_names()
|
||||||
|
assert names == sorted(names)
|
||||||
|
|
||||||
|
|
||||||
|
class TestGetProfile:
|
||||||
|
def test_returns_profile_instance(self):
|
||||||
|
p = get_profile("minimal")
|
||||||
|
assert isinstance(p, Profile)
|
||||||
|
|
||||||
|
def test_unknown_raises_value_error(self):
|
||||||
|
with pytest.raises(ValueError, match="Unknown launch profile"):
|
||||||
|
get_profile("turbo")
|
||||||
|
|
||||||
|
def test_error_message_lists_valid_profiles(self):
|
||||||
|
with pytest.raises(ValueError) as exc_info:
|
||||||
|
get_profile("bogus")
|
||||||
|
msg = str(exc_info.value)
|
||||||
|
assert "debug" in msg
|
||||||
|
assert "full" in msg
|
||||||
|
assert "minimal" in msg
|
||||||
|
|
||||||
|
def test_get_minimal_name(self):
|
||||||
|
assert get_profile("minimal").name == "minimal"
|
||||||
|
|
||||||
|
def test_get_full_name(self):
|
||||||
|
assert get_profile("full").name == "full"
|
||||||
|
|
||||||
|
def test_get_debug_name(self):
|
||||||
|
assert get_profile("debug").name == "debug"
|
||||||
|
|
||||||
|
|
||||||
|
# ─── Minimal profile ──────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
class TestMinimalProfile:
|
||||||
|
def setup_method(self):
|
||||||
|
self.p = _minimal()
|
||||||
|
|
||||||
|
def test_name(self):
|
||||||
|
assert self.p.name == "minimal"
|
||||||
|
|
||||||
|
def test_drivers_enabled(self):
|
||||||
|
assert self.p.enable_stm32_bridge is True
|
||||||
|
assert self.p.enable_sensors is True
|
||||||
|
assert self.p.enable_motor_daemon is True
|
||||||
|
assert self.p.enable_imu is True
|
||||||
|
|
||||||
|
def test_perception_disabled(self):
|
||||||
|
assert self.p.enable_uwb is False
|
||||||
|
assert self.p.enable_person_detection is False
|
||||||
|
assert self.p.enable_object_detection is False
|
||||||
|
assert self.p.enable_depth_costmap is False
|
||||||
|
assert self.p.enable_gimbal is False
|
||||||
|
|
||||||
|
def test_navigation_disabled(self):
|
||||||
|
assert self.p.enable_slam is False
|
||||||
|
assert self.p.enable_nav2 is False
|
||||||
|
assert self.p.enable_follower is False
|
||||||
|
assert self.p.enable_docking is False
|
||||||
|
assert self.p.enable_lidar_avoid is False
|
||||||
|
|
||||||
|
def test_ui_disabled(self):
|
||||||
|
assert self.p.enable_rosbridge is False
|
||||||
|
assert self.p.enable_audio is False
|
||||||
|
assert self.p.enable_social is False
|
||||||
|
assert self.p.enable_csi_cameras is False
|
||||||
|
|
||||||
|
def test_debug_features_disabled(self):
|
||||||
|
assert self.p.enable_rviz is False
|
||||||
|
assert self.p.enable_verbose_logs is False
|
||||||
|
assert self.p.enable_bag_recorder is False
|
||||||
|
|
||||||
|
def test_timing_zero(self):
|
||||||
|
# Unused timings are zeroed so there are no unnecessary waits
|
||||||
|
assert self.p.t_perception == 0.0
|
||||||
|
assert self.p.t_navigation == 0.0
|
||||||
|
assert self.p.t_ui == 0.0
|
||||||
|
|
||||||
|
def test_diagnostics_enabled(self):
|
||||||
|
assert self.p.enable_diagnostics is True
|
||||||
|
|
||||||
|
|
||||||
|
# ─── Full profile ─────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
class TestFullProfile:
|
||||||
|
def setup_method(self):
|
||||||
|
self.p = _full()
|
||||||
|
|
||||||
|
def test_name(self):
|
||||||
|
assert self.p.name == "full"
|
||||||
|
|
||||||
|
def test_drivers_enabled(self):
|
||||||
|
assert self.p.enable_stm32_bridge is True
|
||||||
|
assert self.p.enable_sensors is True
|
||||||
|
assert self.p.enable_motor_daemon is True
|
||||||
|
assert self.p.enable_imu is True
|
||||||
|
|
||||||
|
def test_perception_enabled(self):
|
||||||
|
assert self.p.enable_uwb is True
|
||||||
|
assert self.p.enable_person_detection is True
|
||||||
|
assert self.p.enable_object_detection is True
|
||||||
|
assert self.p.enable_depth_costmap is True
|
||||||
|
assert self.p.enable_gimbal is True
|
||||||
|
|
||||||
|
def test_navigation_enabled(self):
|
||||||
|
assert self.p.enable_slam is True
|
||||||
|
assert self.p.enable_nav2 is True
|
||||||
|
assert self.p.enable_follower is True
|
||||||
|
assert self.p.enable_docking is True
|
||||||
|
assert self.p.enable_lidar_avoid is True
|
||||||
|
|
||||||
|
def test_ui_enabled(self):
|
||||||
|
assert self.p.enable_rosbridge is True
|
||||||
|
assert self.p.enable_audio is True
|
||||||
|
assert self.p.enable_social is True
|
||||||
|
assert self.p.enable_csi_cameras is True
|
||||||
|
|
||||||
|
def test_diagnostics_enabled(self):
|
||||||
|
assert self.p.enable_diagnostics is True
|
||||||
|
|
||||||
|
def test_debug_features_disabled(self):
|
||||||
|
assert self.p.enable_rviz is False
|
||||||
|
assert self.p.enable_verbose_logs is False
|
||||||
|
|
||||||
|
def test_timing_positive(self):
|
||||||
|
assert self.p.t_drivers == 0.0
|
||||||
|
assert self.p.t_perception > 0.0
|
||||||
|
assert self.p.t_navigation > self.p.t_perception
|
||||||
|
assert self.p.t_ui > self.p.t_navigation
|
||||||
|
|
||||||
|
def test_perception_gate_8s(self):
|
||||||
|
assert self.p.t_perception == 8.0
|
||||||
|
|
||||||
|
def test_navigation_gate_16s(self):
|
||||||
|
assert self.p.t_navigation == 16.0
|
||||||
|
|
||||||
|
def test_ui_gate_22s(self):
|
||||||
|
assert self.p.t_ui == 22.0
|
||||||
|
|
||||||
|
|
||||||
|
# ─── Debug profile ────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
class TestDebugProfile:
|
||||||
|
def setup_method(self):
|
||||||
|
self.p = _debug()
|
||||||
|
|
||||||
|
def test_name(self):
|
||||||
|
assert self.p.name == "debug"
|
||||||
|
|
||||||
|
def test_inherits_full_stack(self):
|
||||||
|
full = _full()
|
||||||
|
# All full flags must be True in debug too
|
||||||
|
assert self.p.enable_slam == full.enable_slam
|
||||||
|
assert self.p.enable_nav2 == full.enable_nav2
|
||||||
|
assert self.p.enable_rosbridge == full.enable_rosbridge
|
||||||
|
|
||||||
|
def test_rviz_enabled(self):
|
||||||
|
assert self.p.enable_rviz is True
|
||||||
|
|
||||||
|
def test_verbose_logs_enabled(self):
|
||||||
|
assert self.p.enable_verbose_logs is True
|
||||||
|
|
||||||
|
def test_bag_recorder_enabled(self):
|
||||||
|
assert self.p.enable_bag_recorder is True
|
||||||
|
|
||||||
|
def test_timing_slower_than_full(self):
|
||||||
|
full = _full()
|
||||||
|
# Debug extends gates to ensure stability
|
||||||
|
assert self.p.t_navigation >= full.t_navigation
|
||||||
|
assert self.p.t_ui >= full.t_ui
|
||||||
|
|
||||||
|
def test_navigation_gate_20s(self):
|
||||||
|
assert self.p.t_navigation == 20.0
|
||||||
|
|
||||||
|
def test_ui_gate_26s(self):
|
||||||
|
assert self.p.t_ui == 26.0
|
||||||
|
|
||||||
|
|
||||||
|
# ─── Profile hierarchy checks ─────────────────────────────────────────────────
|
||||||
|
|
||||||
|
class TestProfileHierarchy:
|
||||||
|
"""Minimal ⊂ Full ⊂ Debug — every flag true in minimal must be true in full/debug."""
|
||||||
|
|
||||||
|
def _enabled_flags(self, p: Profile) -> set:
|
||||||
|
import dataclasses
|
||||||
|
return {
|
||||||
|
f.name
|
||||||
|
for f in dataclasses.fields(p)
|
||||||
|
if f.name.startswith("enable_") and getattr(p, f.name) is True
|
||||||
|
}
|
||||||
|
|
||||||
|
def test_minimal_subset_of_full(self):
|
||||||
|
minimal_flags = self._enabled_flags(_minimal())
|
||||||
|
full_flags = self._enabled_flags(_full())
|
||||||
|
assert minimal_flags.issubset(full_flags), (
|
||||||
|
f"Full missing flags that Minimal has: {minimal_flags - full_flags}"
|
||||||
|
)
|
||||||
|
|
||||||
|
def test_full_subset_of_debug(self):
|
||||||
|
full_flags = self._enabled_flags(_full())
|
||||||
|
debug_flags = self._enabled_flags(_debug())
|
||||||
|
assert full_flags.issubset(debug_flags), (
|
||||||
|
f"Debug missing flags that Full has: {full_flags - debug_flags}"
|
||||||
|
)
|
||||||
|
|
||||||
|
def test_debug_has_extra_flags(self):
|
||||||
|
full_flags = self._enabled_flags(_full())
|
||||||
|
debug_flags = self._enabled_flags(_debug())
|
||||||
|
extras = debug_flags - full_flags
|
||||||
|
assert len(extras) > 0, "Debug should have at least one extra flag over Full"
|
||||||
|
|
||||||
|
def test_debug_extras_are_debug_features(self):
|
||||||
|
full_flags = self._enabled_flags(_full())
|
||||||
|
debug_flags = self._enabled_flags(_debug())
|
||||||
|
extras = debug_flags - full_flags
|
||||||
|
for flag in extras:
|
||||||
|
assert "rviz" in flag or "verbose" in flag or "bag" in flag or "debug" in flag, (
|
||||||
|
f"Unexpected extra flag in debug: {flag}"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# ─── to_dict ──────────────────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
class TestToDict:
|
||||||
|
def test_returns_dict(self):
|
||||||
|
p = _minimal()
|
||||||
|
d = p.to_dict()
|
||||||
|
assert isinstance(d, dict)
|
||||||
|
|
||||||
|
def test_name_in_dict(self):
|
||||||
|
d = _minimal().to_dict()
|
||||||
|
assert d["name"] == "minimal"
|
||||||
|
|
||||||
|
def test_all_bool_flags_present(self):
|
||||||
|
d = _full().to_dict()
|
||||||
|
for key in ("enable_slam", "enable_nav2", "enable_rosbridge", "enable_rviz"):
|
||||||
|
assert key in d, f"Missing key: {key}"
|
||||||
|
|
||||||
|
def test_timing_fields_present(self):
|
||||||
|
d = _full().to_dict()
|
||||||
|
for key in ("t_drivers", "t_perception", "t_navigation", "t_ui"):
|
||||||
|
assert key in d, f"Missing timing key: {key}"
|
||||||
|
|
||||||
|
def test_values_are_native_types(self):
|
||||||
|
d = _debug().to_dict()
|
||||||
|
for v in d.values():
|
||||||
|
assert isinstance(v, (bool, int, float, str)), (
|
||||||
|
f"Expected native type, got {type(v)} for value {v!r}"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# ─── Safety parameter defaults ────────────────────────────────────────────────
|
||||||
|
|
||||||
|
class TestSafetyDefaults:
|
||||||
|
def test_watchdog_timeout_positive(self):
|
||||||
|
for name in profile_names():
|
||||||
|
p = get_profile(name)
|
||||||
|
assert p.watchdog_timeout_s > 0, f"{name}: watchdog_timeout_s must be > 0"
|
||||||
|
|
||||||
|
def test_max_linear_vel_bounded(self):
|
||||||
|
for name in profile_names():
|
||||||
|
p = get_profile(name)
|
||||||
|
assert 0 < p.max_linear_vel <= 2.0, (
|
||||||
|
f"{name}: max_linear_vel {p.max_linear_vel} outside safe range"
|
||||||
|
)
|
||||||
|
|
||||||
|
def test_follow_distance_positive(self):
|
||||||
|
for name in profile_names():
|
||||||
|
p = get_profile(name)
|
||||||
|
assert p.follow_distance_m > 0, f"{name}: follow_distance_m must be > 0"
|
||||||
|
|
||||||
|
def test_cmd_vel_deadman_positive(self):
|
||||||
|
for name in profile_names():
|
||||||
|
p = get_profile(name)
|
||||||
|
assert p.cmd_vel_deadman_s > 0, f"{name}: cmd_vel_deadman_s must be > 0"
|
||||||
|
|
||||||
|
|
||||||
|
# ─── Hardware port defaults ────────────────────────────────────────────────────
|
||||||
|
|
||||||
|
class TestHardwarePortDefaults:
|
||||||
|
def test_stm32_port_set(self):
|
||||||
|
p = _minimal()
|
||||||
|
assert p.stm32_port.startswith("/dev/")
|
||||||
|
|
||||||
|
def test_uwb_ports_set(self):
|
||||||
|
p = _full()
|
||||||
|
assert p.uwb_port_a.startswith("/dev/")
|
||||||
|
assert p.uwb_port_b.startswith("/dev/")
|
||||||
|
|
||||||
|
def test_gimbal_port_set(self):
|
||||||
|
p = _full()
|
||||||
|
assert p.gimbal_port.startswith("/dev/")
|
||||||
Loading…
x
Reference in New Issue
Block a user