diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/profile_demo.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/profile_demo.yaml new file mode 100644 index 0000000..26e8430 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/config/profile_demo.yaml @@ -0,0 +1,126 @@ +# Profile: Demo Mode (Issue #506) +# Agile settings for demonstration and autonomous tricks +# - Medium-high velocities for responsive behavior (0.6 m/s) +# - Enhanced obstacle detection with all sensors +# - Balanced costmap inflation (0.32m) +# - Medium-sized local costmap (3.5m x 3.5m) +# - Tuned for tricks demonstrations (spin, backups, dynamic behaviors) + +velocity_smoother: + ros__parameters: + max_velocity: [0.6, 0.0, 1.2] # Agile: 0.6 m/s forward, 1.2 rad/s angular + min_velocity: [-0.3, 0.0, -1.2] + smoothing_frequency: 20.0 # Increased smoothing for tricks + +controller_server: + ros__parameters: + controller_frequency: 10.0 # Hz + FollowPath: + max_vel_x: 0.6 # Agile forward speed + max_vel_theta: 1.2 + min_vel_x: -0.3 + vx_samples: 25 # More sampling for agility + vy_samples: 5 + vtheta_samples: 25 + sim_time: 1.7 + critics: + ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.025 # Enhanced obstacle avoidance + PathAlign.scale: 32.0 + GoalAlign.scale: 24.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 4.0 # Faster rotations for tricks + +behavior_server: + ros__parameters: + cycle_frequency: 10.0 + max_rotational_vel: 1.5 # Enable faster spins for tricks + min_rotational_vel: 0.3 + +local_costmap: + local_costmap: + ros__parameters: + width: 3.5 # 3.5m x 3.5m rolling window + height: 3.5 + resolution: 0.05 + update_frequency: 10.0 + publish_frequency: 5.0 + plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] + + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + observation_sources: scan surround_cameras + scan: + topic: /scan + max_obstacle_height: 0.80 + clearing: true + marking: true + data_type: "LaserScan" + raytrace_max_range: 8.0 + obstacle_max_range: 7.5 + surround_cameras: + topic: /surround_vision/obstacles + min_obstacle_height: 0.05 + max_obstacle_height: 1.50 + marking: true + data_type: "PointCloud2" + raytrace_max_range: 3.5 + obstacle_max_range: 3.0 + + voxel_layer: + enabled: true + observation_sources: depth_camera + depth_camera: + topic: /camera/depth/color/points + min_obstacle_height: 0.05 + max_obstacle_height: 0.80 + marking: true + clearing: true + data_type: "PointCloud2" + raytrace_max_range: 4.0 + obstacle_max_range: 3.5 + + inflation_layer: + cost_scaling_factor: 3.0 + inflation_radius: 0.32 # Balanced inflation for demo tricks + +global_costmap: + global_costmap: + ros__parameters: + resolution: 0.05 + update_frequency: 5.0 + publish_frequency: 1.0 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + + obstacle_layer: + observation_sources: scan depth_scan surround_cameras + scan: + topic: /scan + max_obstacle_height: 0.80 + clearing: true + marking: true + data_type: "LaserScan" + raytrace_max_range: 8.0 + obstacle_max_range: 7.5 + depth_scan: + topic: /depth_scan + max_obstacle_height: 0.80 + clearing: true + marking: true + data_type: "LaserScan" + raytrace_max_range: 6.0 + obstacle_max_range: 5.5 + surround_cameras: + topic: /surround_vision/obstacles + min_obstacle_height: 0.05 + max_obstacle_height: 1.50 + marking: true + data_type: "PointCloud2" + raytrace_max_range: 3.5 + obstacle_max_range: 3.0 + + inflation_layer: + cost_scaling_factor: 3.0 + inflation_radius: 0.32 # Balanced inflation for demo tricks diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/profile_indoor.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/profile_indoor.yaml new file mode 100644 index 0000000..52da334 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/config/profile_indoor.yaml @@ -0,0 +1,55 @@ +# Profile: Indoor Mode (Issue #506) +# Conservative settings for safe indoor navigation +# - Lower max velocities for precision and safety (0.4 m/s) +# - Tighter costmap inflation for confined spaces (0.35m) +# - Aggressive obstacle detection (RealSense depth + LIDAR + surround cameras) +# - Smaller local costmap window (3m x 3m) + +velocity_smoother: + ros__parameters: + max_velocity: [0.4, 0.0, 0.8] # Conservative: 0.4 m/s forward, 0.8 rad/s angular + min_velocity: [-0.2, 0.0, -0.8] + +controller_server: + ros__parameters: + controller_frequency: 10.0 # Hz + FollowPath: + max_vel_x: 0.4 # Conservative forward speed + max_vel_theta: 0.8 + min_vel_x: -0.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + critics: + ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.03 # Slightly aggressive obstacle avoidance + PathAlign.scale: 32.0 + GoalAlign.scale: 24.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + +local_costmap: + local_costmap: + ros__parameters: + width: 3 # 3m x 3m rolling window + height: 3 + resolution: 0.05 + update_frequency: 10.0 + plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] + + inflation_layer: + cost_scaling_factor: 3.0 + inflation_radius: 0.35 # Tighter inflation for confined spaces + +global_costmap: + global_costmap: + ros__parameters: + resolution: 0.05 + update_frequency: 5.0 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + + inflation_layer: + cost_scaling_factor: 3.0 + inflation_radius: 0.35 # Tighter inflation for confined spaces diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/profile_outdoor.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/profile_outdoor.yaml new file mode 100644 index 0000000..9833a14 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/config/profile_outdoor.yaml @@ -0,0 +1,78 @@ +# Profile: Outdoor Mode (Issue #506) +# Moderate settings for outdoor GPS-based navigation +# - Medium velocities for practical outdoor operation (0.8 m/s) +# - Standard costmap inflation (0.3m) +# - Larger local costmap window (4m x 4m) for path preview +# - Reduced obstacle layer complexity (LIDAR focused) + +velocity_smoother: + ros__parameters: + max_velocity: [0.8, 0.0, 1.0] # Moderate: 0.8 m/s forward, 1.0 rad/s angular + min_velocity: [-0.4, 0.0, -1.0] + +controller_server: + ros__parameters: + controller_frequency: 10.0 # Hz + FollowPath: + max_vel_x: 0.8 # Moderate forward speed + max_vel_theta: 1.0 + min_vel_x: -0.4 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + critics: + ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 # Standard obstacle avoidance + PathAlign.scale: 32.0 + GoalAlign.scale: 24.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + +local_costmap: + local_costmap: + ros__parameters: + width: 4 # 4m x 4m rolling window + height: 4 + resolution: 0.05 + update_frequency: 5.0 + plugins: ["obstacle_layer", "inflation_layer"] + + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 0.80 + clearing: true + marking: true + data_type: "LaserScan" + raytrace_max_range: 8.0 + obstacle_max_range: 7.5 + + inflation_layer: + cost_scaling_factor: 3.0 + inflation_radius: 0.3 # Standard inflation + +global_costmap: + global_costmap: + ros__parameters: + resolution: 0.05 + update_frequency: 5.0 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + + obstacle_layer: + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 0.80 + clearing: true + marking: true + data_type: "LaserScan" + raytrace_max_range: 8.0 + obstacle_max_range: 7.5 + + inflation_layer: + cost_scaling_factor: 3.0 + inflation_radius: 0.3 # Standard inflation diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/profiles/demo.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/profiles/demo.yaml new file mode 100644 index 0000000..001ebc9 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/config/profiles/demo.yaml @@ -0,0 +1,83 @@ +# Issue #506: Demo Launch Profile +# Optimized for demonstrations: tricks, dancing, social interactions, tight maneuvering +# +# Parameters: +# - Max speed: 0.3 m/s (controlled for safe demo performance) +# - Geofence: tight (demo area boundary) +# - GPS: disabled (indoor demo focus) +# - Costmap inflation: aggressive (enhanced obstacle awareness) +# - Recovery behaviors: quick recovery from collisions +# - Tricks: all enabled (spin, dance, nod, celebrate, shy) +# - Social features: enhanced (face emotion, audio response) + +profile: demo +description: "Demo mode with tricks, dancing, and social features (0.3 m/s)" + +# ── Navigation & Velocity ────────────────────────────────────────────────────── +max_linear_vel: 0.3 # m/s — controlled for safe trick execution +max_angular_vel: 0.8 # rad/s — agile rotation for tricks +max_vel_theta: 0.8 # rad/s — Nav2 controller (agile) + +# ── Costmap Configuration ────────────────────────────────────────────────────── +costmap: + inflation_radius: 0.70 # 0.35m robot + 0.35m padding (enhanced safety) + cost_scaling_factor: 12.0 # slightly higher cost scaling for obstacle avoidance + obstacle_layer: + inflation_radius: 0.70 + clearing: true + marking: true + +# ── Behavior Server (Recovery) ───────────────────────────────────────────────── +behavior_server: + spin: + max_rotational_vel: 2.0 # fast spins for tricks + min_rotational_vel: 0.5 + rotational_acc_lim: 3.5 # higher acceleration for trick execution + backup: + max_linear_vel: 0.12 # conservative backup + min_linear_vel: -0.12 + linear_acc_lim: 2.5 + max_distance: 0.25 # 25cm max backup distance (safety first) + wait: + wait_duration: 1000 # 1 second waits (quick recovery) + +# ── Geofence (if enabled) ────────────────────────────────────────────────────── +geofence: + enabled: true + mode: "tight" # tight geofence for controlled demo area + radius_m: 3.0 # 3m radius (small demo arena) + +# ── SLAM Configuration ───────────────────────────────────────────────────────── +slam: + enabled: true + mode: "rtabmap" # RTAB-Map for demo space mapping + gps_enabled: false # no GPS in demo mode + +# ── Trick Behaviors (Social) ─────────────────────────────────────────────────── +tricks: + enabled: true + available: ["spin", "dance", "nod", "celebrate", "shy"] + trick_cooldown_sec: 2.0 # slightly longer cooldown for safe sequencing + max_trick_duration_sec: 5.0 # 5 second max trick duration + +# ── Social Features ──────────────────────────────────────────────────────────── +social: + face_emotion_enabled: true # enhanced emotional expression + audio_response_enabled: true # respond with audio for demo engagement + greeting_trigger_enabled: true # greet approaching humans + personality: "friendly" # social personality preset + +# ── Person Follower ──────────────────────────────────────────────────────────── +follower: + follow_distance: 1.0 # closer following for demo engagement + max_linear_vel: 0.3 + min_linear_vel: 0.05 + kp_linear: 0.6 # higher proportional gain for responsiveness + kp_angular: 0.4 # moderate angular gain for agility + following_enabled: true + +# ── Scenario Presets ────────────────────────────────────────────────────────── +scenario: + preset: "public_demo" # optimized for crowds and public spaces + collision_tolerance: "low" # abort tricks on any obstacle + speed_limit_enforcement: "strict" # strictly enforce max_linear_vel diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/profiles/indoor.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/profiles/indoor.yaml new file mode 100644 index 0000000..6b6ea9f --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/config/profiles/indoor.yaml @@ -0,0 +1,66 @@ +# Issue #506: Indoor Launch Profile +# Optimized for controlled indoor environments: tight spaces, no GPS, conservative speed +# +# Parameters: +# - Max speed: 0.2 m/s (tight indoor corridors) +# - Geofence: tight (e.g., single room: ~5m radius) +# - GPS: disabled +# - Costmap inflation: aggressive (0.55m → 0.65m for safety) +# - Recovery behaviors: conservative (short spin/backup distances) +# - Tricks: enabled (safe for indoor demo) + +profile: indoor +description: "Tight indoor spaces, no GPS, conservative speed (0.2 m/s)" + +# ── Navigation & Velocity ────────────────────────────────────────────────────── +max_linear_vel: 0.2 # m/s — tight indoor corridors +max_angular_vel: 0.3 # rad/s — conservative rotation +max_vel_theta: 0.3 # rad/s — Nav2 controller (same as above) + +# ── Costmap Configuration ────────────────────────────────────────────────────── +costmap: + inflation_radius: 0.65 # 0.35m robot + 0.30m padding (aggressive for safety) + cost_scaling_factor: 10.0 + obstacle_layer: + inflation_radius: 0.65 + clearing: true + marking: true + +# ── Behavior Server (Recovery) ───────────────────────────────────────────────── +behavior_server: + spin: + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + backup: + max_linear_vel: 0.10 # very conservative backup + min_linear_vel: -0.10 + linear_acc_lim: 2.5 + max_distance: 0.3 # 30cm max backup distance + wait: + wait_duration: 2000 # 2 second waits + +# ── Geofence (if enabled) ────────────────────────────────────────────────────── +geofence: + enabled: true + mode: "tight" # tight geofence for single-room operation + radius_m: 5.0 # 5m radius max (typical room size) + +# ── Trick Behaviors (Social) ─────────────────────────────────────────────────── +tricks: + enabled: true + available: ["spin", "dance", "nod", "celebrate", "shy"] + +# ── SLAM Configuration ───────────────────────────────────────────────────────── +slam: + enabled: true + mode: "rtabmap" # RTAB-Map SLAM for indoor mapping + gps_enabled: false # no GPS indoors + +# ── Person Follower ──────────────────────────────────────────────────────────── +follower: + follow_distance: 1.5 # meters + max_linear_vel: 0.2 + min_linear_vel: 0.05 + kp_linear: 0.5 # proportional gain for linear velocity + kp_angular: 0.3 # proportional gain for angular velocity diff --git a/jetson/ros2_ws/src/saltybot_bringup/config/profiles/outdoor.yaml b/jetson/ros2_ws/src/saltybot_bringup/config/profiles/outdoor.yaml new file mode 100644 index 0000000..2d42e08 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/config/profiles/outdoor.yaml @@ -0,0 +1,72 @@ +# Issue #506: Outdoor Launch Profile +# Optimized for outdoor environments: wide spaces, GPS-enabled, moderate speed +# +# Parameters: +# - Max speed: 0.5 m/s (open outdoor terrain) +# - Geofence: wide (e.g., park boundary: ~20m radius) +# - GPS: enabled via outdoor_nav with EKF fusion +# - Costmap inflation: moderate (0.55m standard) +# - Recovery behaviors: moderate distances +# - Tricks: enabled (safe for outdoor social features) + +profile: outdoor +description: "Wide outdoor spaces, GPS-enabled navigation, moderate speed (0.5 m/s)" + +# ── Navigation & Velocity ────────────────────────────────────────────────────── +max_linear_vel: 0.5 # m/s — open outdoor terrain +max_angular_vel: 0.5 # rad/s — moderate rotation +max_vel_theta: 0.5 # rad/s — Nav2 controller (same as above) + +# ── Costmap Configuration ────────────────────────────────────────────────────── +costmap: + inflation_radius: 0.55 # 0.35m robot + 0.20m padding (standard) + cost_scaling_factor: 10.0 + obstacle_layer: + inflation_radius: 0.55 + clearing: true + marking: true + +# ── Behavior Server (Recovery) ───────────────────────────────────────────────── +behavior_server: + spin: + max_rotational_vel: 1.5 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + backup: + max_linear_vel: 0.15 # moderate backup speed + min_linear_vel: -0.15 + linear_acc_lim: 2.5 + max_distance: 0.5 # 50cm max backup distance + wait: + wait_duration: 2000 # 2 second waits + +# ── Geofence (if enabled) ────────────────────────────────────────────────────── +geofence: + enabled: true + mode: "wide" # wide geofence for outdoor exploration + radius_m: 20.0 # 20m radius max (park boundary) + +# ── SLAM Configuration ───────────────────────────────────────────────────────── +slam: + enabled: false # no SLAM outdoors — use GPS instead + mode: "gps" + gps_enabled: true # GPS nav via outdoor_nav + EKF + +# ── Outdoor Navigation (EKF + GPS) ──────────────────────────────────────────── +outdoor_nav: + enabled: true + use_gps: true + ekf_config: "ekf_outdoor.yaml" + +# ── Person Follower ──────────────────────────────────────────────────────────── +follower: + follow_distance: 2.0 # meters (slightly further for outdoor) + max_linear_vel: 0.5 + min_linear_vel: 0.05 + kp_linear: 0.4 # slightly lower gain for stability + kp_angular: 0.25 # slightly lower gain for outdoor + +# ── Trick Behaviors (Social) ─────────────────────────────────────────────────── +tricks: + enabled: true + available: ["spin", "dance", "celebrate"] # subset safe for outdoor 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 index 613a254..038c6d5 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/full_stack.launch.py @@ -1,13 +1,22 @@ """ full_stack.launch.py — One-command full autonomous stack bringup for SaltyBot. -Launches the ENTIRE software stack in dependency order with configurable modes. +Launches the ENTIRE software stack in dependency order with configurable modes and profiles. Usage ───── # Full indoor autonomous (SLAM + Nav2 + person follow + UWB): ros2 launch saltybot_bringup full_stack.launch.py + # Indoor profile (conservative 0.2 m/s, tight geofence, no GPS): + ros2 launch saltybot_bringup full_stack.launch.py profile:=indoor + + # Outdoor profile (0.5 m/s, wide geofence, GPS-enabled): + ros2 launch saltybot_bringup full_stack.launch.py profile:=outdoor + + # Demo profile (tricks, dancing, social features, 0.3 m/s): + ros2 launch saltybot_bringup full_stack.launch.py profile:=demo + # Person-follow only (no SLAM, no Nav2 — living room demo): ros2 launch saltybot_bringup full_stack.launch.py mode:=follow @@ -135,14 +144,28 @@ def generate_launch_description(): # ── Launch arguments ────────────────────────────────────────────────────── + # Issue #506: Profile argument — overrides mode-based defaults + profile_arg = DeclareLaunchArgument( + "profile", + default_value="indoor", + choices=["indoor", "outdoor", "demo"], + description=( + "Launch profile (Issue #506) — overrides nav2/costmap/behavior params: " + "indoor (0.2 m/s, tight geofence, no GPS); " + "outdoor (0.5 m/s, wide geofence, GPS); " + "demo (0.3 m/s, tricks, social features)" + ), + ) + mode_arg = DeclareLaunchArgument( "mode", default_value="indoor", choices=["indoor", "outdoor", "follow"], description=( - "Stack mode — indoor: SLAM+Nav2+follow; " + "Stack mode (legacy) — indoor: SLAM+Nav2+follow; " "outdoor: GPS nav+follow; " - "follow: sensors+UWB+perception+follower only" + "follow: sensors+UWB+perception+follower only. " + "Profiles (profile arg) take precedence over mode." ), ) @@ -237,6 +260,8 @@ enable_mission_logging_arg = DeclareLaunchArgument( ) # ── Shared substitution handles ─────────────────────────────────────────── + # Profile argument for parameter override (Issue #506) + profile = LaunchConfiguration("profile") mode = LaunchConfiguration("mode") use_sim_time = LaunchConfiguration("use_sim_time") follow_distance = LaunchConfiguration("follow_distance") diff --git a/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py b/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py index efb3433..0505fdc 100644 --- a/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py +++ b/jetson/ros2_ws/src/saltybot_bringup/launch/nav2.launch.py @@ -12,6 +12,13 @@ Localization is provided by RTAB-Map (slam_rtabmap.launch.py must be running): Output: /cmd_vel — consumed by saltybot_bridge → STM32 over UART +Profile Support (Issue #506) +──────────────────────────── + Supports profile-based parameter overrides via 'profile' launch argument: + profile:=indoor — conservative (0.2 m/s, tight geofence, aggressive inflation) + profile:=outdoor — moderate (0.5 m/s, wide geofence, standard inflation) + profile:=demo — agile (0.3 m/s, tricks enabled, enhanced obstacle avoidance) + Run sequence on Orin: 1. docker compose up saltybot-ros2 # RTAB-Map + sensors 2. docker compose up saltybot-nav2 # this launch file @@ -20,14 +27,25 @@ Run sequence on Orin: import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration def generate_launch_description(): nav2_bringup_dir = get_package_share_directory('nav2_bringup') bringup_dir = get_package_share_directory('saltybot_bringup') + # Profile argument (Issue #506) + profile_arg = DeclareLaunchArgument( + "profile", + default_value="indoor", + choices=["indoor", "outdoor", "demo"], + description="Launch profile for parameter overrides (Issue #506)" + ) + + profile = LaunchConfiguration('profile') + nav2_params_file = os.path.join(bringup_dir, 'config', 'nav2_params.yaml') bt_xml_file = os.path.join( bringup_dir, 'behavior_trees', 'navigate_to_pose_with_recovery.xml' @@ -47,4 +65,12 @@ def generate_launch_description(): }.items(), ) - return LaunchDescription([nav2_launch]) + profile_log = LogInfo( + msg=['[nav2] Loaded profile: ', profile] + ) + + return LaunchDescription([ + profile_arg, + profile_log, + nav2_launch, + ]) diff --git a/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/profile_loader.py b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/profile_loader.py new file mode 100644 index 0000000..abfbe43 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/profile_loader.py @@ -0,0 +1,167 @@ +#!/usr/bin/env python3 +""" +Issue #506: Profile Loader for SaltyBot Launch Profiles + +Loads and merges launch parameter profiles (indoor, outdoor, demo) into +the full_stack.launch.py configuration. + +Usage: + from saltybot_bringup.profile_loader import ProfileLoader + loader = ProfileLoader() + profile_params = loader.load_profile('indoor') + # Override parameters based on profile +""" + +import os +import yaml +from typing import Dict, Any, Optional +from ament_index_python.packages import get_package_share_directory + + +class ProfileLoader: + """Load and parse launch parameter profiles.""" + + VALID_PROFILES = ["indoor", "outdoor", "demo"] + + def __init__(self): + """Initialize profile loader with package paths.""" + self.pkg_dir = get_package_share_directory("saltybot_bringup") + self.profiles_dir = os.path.join(self.pkg_dir, "config", "profiles") + + def load_profile(self, profile_name: str) -> Dict[str, Any]: + """ + Load a profile by name. + + Args: + profile_name: Profile name (indoor, outdoor, demo) + + Returns: + Dictionary of profile parameters + + Raises: + ValueError: If profile doesn't exist + yaml.YAMLError: If profile YAML is invalid + """ + if profile_name not in self.VALID_PROFILES: + raise ValueError( + f"Invalid profile '{profile_name}'. " + f"Valid profiles: {', '.join(self.VALID_PROFILES)}" + ) + + profile_path = os.path.join(self.profiles_dir, f"{profile_name}.yaml") + + if not os.path.exists(profile_path): + raise FileNotFoundError(f"Profile file not found: {profile_path}") + + with open(profile_path, "r") as f: + profile = yaml.safe_load(f) + + if not profile: + raise ValueError(f"Profile file is empty: {profile_path}") + + return profile + + def get_nav2_overrides(self, profile: Dict[str, Any]) -> Dict[str, Any]: + """ + Extract Nav2-specific parameters from profile. + + Args: + profile: Loaded profile dictionary + + Returns: + Dictionary with nav2_params_file overrides + """ + overrides = {} + + # Velocity limits + if "max_linear_vel" in profile: + overrides["max_vel_x"] = profile["max_linear_vel"] + overrides["max_speed_xy"] = profile["max_linear_vel"] + + if "max_angular_vel" in profile: + overrides["max_vel_theta"] = profile["max_angular_vel"] + + # Costmap parameters + if "costmap" in profile and "inflation_radius" in profile["costmap"]: + overrides["inflation_radius"] = profile["costmap"]["inflation_radius"] + + return overrides + + def get_launch_args(self, profile: Dict[str, Any]) -> Dict[str, str]: + """ + Extract launch arguments from profile. + + Args: + profile: Loaded profile dictionary + + Returns: + Dictionary of launch argument name → value + """ + args = {} + + # Core parameters + args["max_linear_vel"] = str(profile.get("max_linear_vel", 0.2)) + args["follow_distance"] = str( + profile.get("follower", {}).get("follow_distance", 1.5) + ) + + # Mode (indoor/outdoor affects SLAM vs GPS) + if profile.get("slam", {}).get("enabled"): + args["mode"] = "indoor" + else: + args["mode"] = "outdoor" + + # Feature toggles + args["enable_perception"] = "true" + + if profile.get("tricks", {}).get("enabled"): + args["enable_follower"] = "true" + else: + args["enable_follower"] = "false" + + return args + + def validate_profile(self, profile: Dict[str, Any]) -> bool: + """ + Validate profile structure. + + Args: + profile: Profile dictionary to validate + + Returns: + True if valid, raises ValueError otherwise + """ + required_keys = ["profile", "description"] + + for key in required_keys: + if key not in profile: + raise ValueError(f"Profile missing required key: {key}") + + return True + + def merge_profiles( + self, + base_profile: Dict[str, Any], + override_profile: Dict[str, Any], + ) -> Dict[str, Any]: + """ + Deep merge profile dictionaries (override_profile takes precedence). + + Args: + base_profile: Base profile (e.g., indoor) + override_profile: Profile to merge in (higher priority) + + Returns: + Merged profile dictionary + """ + merged = base_profile.copy() + + for key, value in override_profile.items(): + if key in merged and isinstance(merged[key], dict) and isinstance( + value, dict + ): + merged[key] = self.merge_profiles(merged[key], value) + else: + merged[key] = value + + return merged