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/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