jetson/ros2_ws/src/saltybot_bringup/config/saltybot_params.yaml

@@ -1,181 +0,0 @@
-# Consolidated SaltyBot parameters (Issue #447)
-# Master configuration for full stack bringup
-
-# ────────────────────────────────────────────────────────────────────────────
-# HARDWARE — STM32 Bridge & Motor Control
-# ────────────────────────────────────────────────────────────────────────────
-
-saltybot_bridge_node:
-  ros__parameters:
-    serial_port: "/dev/stm32-bridge"
-    baud_rate: 921600
-    timeout: 0.05
-    reconnect_delay: 2.0
-    heartbeat_period: 0.2
-
-saltybot_cmd_vel_bridge:
-  ros__parameters:
-    speed_scale: 1000.0    # m/s → ESC units
-    steer_scale: -500.0    # rad/s → ESC units
-    deadman_timeout: 0.5   # seconds
-    max_linear_vel: 0.5    # m/s (can be overridden at launch)
-    ramp_rate: 0.2         # m/s²
-
-# ────────────────────────────────────────────────────────────────────────────
-# PERCEPTION — Sensors & Detection
-# ────────────────────────────────────────────────────────────────────────────
-
-rplidar_node:
-  ros__parameters:
-    serial_port: "/dev/rplidar"
-    serial_baudrate: 115200
-    frame_id: "laser"
-    angle_compensate: true
-
-realsense_node:
-  ros__parameters:
-    camera_name: "camera"
-    depth_module:
-      profile: "640x480x30"
-    rgb_camera:
-      profile: "640x480x30"
-
-yolov8_detector:
-  ros__parameters:
-    confidence_threshold: 0.5
-    nms_threshold: 0.4
-    input_shape: [640, 640]
-    model_path: "/opt/models/yolov8n_coco.engine"
-    enable_tracking: true
-
-# ────────────────────────────────────────────────────────────────────────────
-# CONTROL — Navigation & Following
-# ────────────────────────────────────────────────────────────────────────────
-
-person_follower:
-  ros__parameters:
-    follow_distance: 1.5   # meters (can be overridden at launch)
-    max_linear_vel: 0.5    # m/s (can be overridden at launch)
-    max_angular_vel: 1.0   # rad/s
-    proportional_gain: 0.3
-    derivative_gain: 0.1
-    update_rate: 10        # Hz
-
-slam_node:
-  ros__parameters:
-    rtabmap:
-      subscribe_depth: true
-      subscribe_scan: true
-      map_frame_id: "map"
-      odom_frame_id: "odom"
-      base_frame_id: "base_link"
-      approx_sync: true
-      queue_size: 10
-
-nav2:
-  ros__parameters:
-    planner_server:
-      ros__parameters:
-        expected_planner_frequency: 10.0
-        planner_plugins: ["GridBased"]
-    controller_server:
-      ros__parameters:
-        expected_controller_frequency: 20.0
-        controller_plugins: ["FollowPath"]
-
-# ────────────────────────────────────────────────────────────────────────────
-# SOCIAL — Communication & Expression
-# ────────────────────────────────────────────────────────────────────────────
-
-tts_service:
-  ros__parameters:
-    voice_model_path: "/opt/models/en_US-cmu_arctic-medium.onnx"
-    audio_device: "alsa_output.usb-Jabra.0.analog-stereo"
-    speed: 1.0
-    pitch: 1.0
-    volume: 0.8
-
-gesture_recognition:
-  ros__parameters:
-    model_complexity: 0          # 0=lite, 1=full, 2=heavy
-    process_fps: 10.0
-    min_confidence: 0.60
-    pose_enabled: true
-    hands_enabled: true
-
-face_tracking:
-  ros__parameters:
-    kp_pan: 1.5
-    ki_pan: 0.1
-    kd_pan: 0.05
-    kp_tilt: 1.2
-    ki_tilt: 0.1
-    kd_tilt: 0.04
-    pan_limit_deg: 90.0
-    tilt_limit_deg: 30.0
-
-# ────────────────────────────────────────────────────────────────────────────
-# MONITORING — Diagnostics & Logging
-# ────────────────────────────────────────────────────────────────────────────
-
-rosbridge_server:
-  ros__parameters:
-    address: "0.0.0.0"
-    port: 9090
-    max_message_size: 1000000000
-
-health_monitor:
-  ros__parameters:
-    heartbeat_timeout: 5.0
-    expected_nodes:
-      - "robot_state_publisher"
-      - "saltybot_bridge"
-      - "cmd_vel_bridge"
-      - "rplidar_node"
-      - "realsense_node"
-      - "person_detector"
-      - "slam_node"
-      - "nav2"
-      - "person_follower"
-      - "rosbridge_server"
-
-# ────────────────────────────────────────────────────────────────────────────
-# HARDWARE CONFIGURATION — Mechanical Parameters
-# ────────────────────────────────────────────────────────────────────────────
-
-robot_description:
-  ros__parameters:
-    # Chassis dimensions
-    wheel_diameter: 0.165   # meters
-    track_width: 0.365      # meters (center-to-center)
-    wheelbase: 0.270        # meters
-
-    # Motor specs
-    motor_max_rpm: 300
-    motor_gearbox_ratio: 10.0
-
-    # Weight & CoG
-    total_weight: 8.0       # kg
-    cog_x: 0.0              # offset from base_link
-    cog_z: 0.12             # height above ground
-
-# ────────────────────────────────────────────────────────────────────────────
-# LAUNCH PROFILES (configured via full_stack.launch.py)
-# ────────────────────────────────────────────────────────────────────────────
-#
-# full (default)
-#   - All sensors, perception, control, social components
-#   - SLAM + Nav2 for indoor navigation
-#   - Person detection + follower
-#   - CSI cameras + rosbridge
-#
-# minimal
-#   - Hardware only: bridge + basic sensors (RPLIDAR + RealSense)
-#   - No SLAM, no Nav2, no person detection, no follower
-#   - No CSI cameras, no rosbridge
-#
-# headless
-#   - Sensors + control: no CSI cameras
-#   - RPLIDAR + RealSense + UWB
-#   - Limited person detection (no CSI fusion)
-#   - No follower, no rosbridge

jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/stack_state_monitor_node.py

@@ -1,116 +0,0 @@
-"""stack_state_monitor_node.py — ROS2 full stack state monitor (Issue #447).
-
-Monitors the boot sequence and publishes /saltybot/stack_state to track
-stack readiness. Reports: booting → ready → error (on node failures).
-
-Topics
-------
-Publish:
-  /saltybot/stack_state  (std_msgs/String) — "idle"/"booting"/"ready"/"error"
-
-Parameters
-----------
-heartbeat_nodes        list    Expected nodes to monitor (from health_monitor config)
-boot_timeout_s         float   30.0   Timeout for full stack bringup (seconds)
-check_interval_s       float   1.0    State check interval (seconds)
-"""
-
-from __future__ import annotations
-
-import time
-from typing import Set
-
-import rclpy
-from rclpy.node import Node
-from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
-
-from std_msgs.msg import String
-
-
-class StackStateMonitor(Node):
-    """Monitor full stack boot sequence and publish stack state."""
-
-    def __init__(self):
-        super().__init__("stack_state_monitor")
-
-        # ── Parameters ──────────────────────────────────────────────────────
-        self.declare_parameter("boot_timeout_s", 30.0)
-        self.declare_parameter("check_interval_s", 1.0)
-
-        self._boot_timeout = self.get_parameter("boot_timeout_s").value
-        self._check_interval = self.get_parameter("check_interval_s").value
-
-        # ── QoS ─────────────────────────────────────────────────────────────
-        reliable_qos = QoSProfile(
-            reliability=QoSReliabilityPolicy.RELIABLE,
-            history=QoSHistoryPolicy.KEEP_LAST,
-            depth=10
-        )
-
-        # ── Publishers ──────────────────────────────────────────────────────
-        self._state_pub = self.create_publisher(
-            String, "/saltybot/stack_state", reliable_qos
-        )
-
-        # ── State ───────────────────────────────────────────────────────────
-        self._state = "idle"
-        self._boot_start_time = time.time()
-        self._nodes_seen: Set[str] = set()
-
-        # ── Timer ───────────────────────────────────────────────────────────
-        self._check_timer = self.create_timer(self._check_interval, self._check_stack)
-
-        self.get_logger().info(
-            f"stack_state_monitor ready "
-            f"(boot_timeout={self._boot_timeout}s)"
-        )
-        self._publish_state("idle")
-
-    def _check_stack(self) -> None:
-        """Monitor stack boot progress and update state."""
-        elapsed = time.time() - self._boot_start_time
-
-        if self._state == "idle":
-            # Transition to booting on first check
-            self._state = "booting"
-            self._publish_state("booting")
-            self.get_logger().info("Stack booting...")
-
-        elif self._state == "booting":
-            # Check if boot timeout exceeded
-            if elapsed > self._boot_timeout:
-                self._state = "error"
-                self._publish_state("error")
-                self.get_logger().error(
-                    f"Boot timeout exceeded ({elapsed:.1f}s > {self._boot_timeout}s)"
-                )
-            else:
-                # Boot still in progress — check if all critical nodes are up
-                # Note: Full node monitoring could be added via node monitor
-                # For now, transition to ready after ~19s (post-launch diagnostics)
-                if elapsed > 19.0:
-                    self._state = "ready"
-                    self._publish_state("ready")
-                    self.get_logger().info("Stack ready!")
-
-    def _publish_state(self, state: str) -> None:
-        """Publish current stack state."""
-        msg = String()
-        msg.data = state
-        self._state_pub.publish(msg)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = StackStateMonitor()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()

jetson/ros2_ws/src/saltybot_geofence/saltybot_geofence/geofence_node.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
-"""Geofence safety system for SaltyBot.
+"""Geofence safety system for SaltyBot - Issue #441.
 
-Subscribes to /phone/gps and /odom. Enforces circle/polygon geofence.
+GPS + Odometry monitoring with configurable geofence (circle/polygon).
 Three zones: SAFE, WARNING (2m buffer), VIOLATION.
 WARNING: slow + concern face + TTS + amber LED.
 VIOLATION: stop + auto-return + red LED.
@@ -62,7 +62,7 @@ class GeofenceNode(Node):
         self.returning_home = False
         self.home_lat = None
         self.home_lon = None
-        self.get_logger().info(f"Geofence initialized: type={self.geofence_type}")
+        self.get_logger().info(f"Geofence: type={self.geofence_type}, radius={self.circle_radius if self.geofence_type=='circle' else 'polygon'}m")
 
     def _gps_callback(self, msg: NavSatFix):
         if msg.latitude == 0 and msg.longitude == 0:
@@ -72,7 +72,6 @@ class GeofenceNode(Node):
         if self.home_lat is None:
             self.home_lat = msg.latitude
             self.home_lon = msg.longitude
-            self.get_logger().info(f"Home: ({self.home_lat:.6f}, {self.home_lon:.6f})")
         self._update_geofence_state()
 
     def _odom_callback(self, msg: Odometry):
@@ -80,10 +79,7 @@ class GeofenceNode(Node):
 
     def _update_geofence_state(self):
         if self.geofence_type == "circle":
-            distance = self._haversine_distance(
+            distance = self._haversine_distance(self.current_lat, self.current_lon, self.circle_center_lat, self.circle_center_lon)
-                self.current_lat, self.current_lon,
-                self.circle_center_lat, self.circle_center_lon
-            )
             if distance <= self.circle_radius - self.warning_distance:
                 new_state = GeofenceState.SAFE
             elif distance <= self.circle_radius:
@@ -113,12 +109,14 @@ class GeofenceNode(Node):
             self.led_pub.publish(String(data="amber"))
             self.emotion_pub.publish(String(data="concerned"))
             if self.tts_enabled:
-                self.tts_pub.publish(String(data=self.get_parameter("tts.warning_message").value))
+                msg = self.get_parameter("tts.warning_message").value
+                self.tts_pub.publish(String(data=msg))
         elif self.current_state == GeofenceState.VIOLATION:
             self.led_pub.publish(String(data="red"))
             self.emotion_pub.publish(String(data="concerned"))
             if self.tts_enabled:
-                self.tts_pub.publish(String(data=self.get_parameter("tts.violation_message").value))
+                msg = self.get_parameter("tts.violation_message").value
+                self.tts_pub.publish(String(data=msg))
             twist = Twist()
             self.cmd_vel_pub.publish(twist)
             if self.auto_return_enabled and not self.returning_home and self.home_lat:
@@ -127,7 +125,6 @@ class GeofenceNode(Node):
         self.last_state = self.current_state
 
     def _initiate_auto_return(self):
-        self.get_logger().warn("Auto-return initiated")
         twist = Twist()
         twist.linear.x = self.auto_return_speed
         self.cmd_vel_pub.publish(twist)
@@ -135,26 +132,20 @@ class GeofenceNode(Node):
     @staticmethod
     def _haversine_distance(lat1, lon1, lat2, lon2):
         R = 6371000
-        phi1 = math.radians(lat1)
+        phi1, phi2 = math.radians(lat1), math.radians(lat2)
-        phi2 = math.radians(lat2)
         delta_phi = math.radians(lat2 - lat1)
         delta_lambda = math.radians(lon2 - lon1)
         a = math.sin(delta_phi/2)**2 + math.cos(phi1) * math.cos(phi2) * math.sin(delta_lambda/2)**2
-        c = 2 * math.asin(math.sqrt(a))
+        return R * 2 * math.asin(math.sqrt(a))
-        return R * c
 
     @staticmethod
     def _point_in_polygon(lat, lon, polygon):
         if not polygon or len(polygon) < 3:
             return False
-        n = len(polygon)
+        inside, p1_lat, p1_lon = False, polygon[0][0], polygon[0][1]
-        inside = False
+        for i in range(1, len(polygon) + 1):
-        p1_lat, p1_lon = polygon[0]
+            p2_lat, p2_lon = polygon[i % len(polygon)]
-        for i in range(1, n + 1):
+            if lat > min(p1_lat, p2_lat) and lat <= max(p1_lat, p2_lat) and lon <= max(p1_lon, p2_lon):
-            p2_lat, p2_lon = polygon[i % n]
-            if lat > min(p1_lat, p2_lat):
-                if lat <= max(p1_lat, p2_lat):
-                    if lon <= max(p1_lon, p2_lon):
                 if p1_lat != p2_lat:
                     xinters = (lat - p1_lat) * (p2_lon - p1_lon) / (p2_lat - p1_lat) + p1_lon
                 if p1_lat == p2_lat or lon <= xinters:
@@ -168,8 +159,7 @@ class GeofenceNode(Node):
             return float("inf")
         min_dist = float("inf")
         for i in range(len(polygon)):
-            p1 = polygon[i]
+            p1, p2 = polygon[i], polygon[(i + 1) % len(polygon)]
-            p2 = polygon[(i + 1) % len(polygon)]
             d = abs((p2[0]-p1[0])*(p1[1]-lon) - (p1[0]-lat)*(p2[1]-p1[1]))
             d /= max(math.sqrt((p2[0]-p1[0])**2 + (p2[1]-p1[1])**2), 1e-6)
             min_dist = min(min_dist, d)
@@ -177,13 +167,11 @@ class GeofenceNode(Node):
 
 def main(args=None):
     rclpy.init(args=args)
-    node = GeofenceNode()
     try:
-        rclpy.spin(node)
+        rclpy.spin(GeofenceNode())
     except KeyboardInterrupt:
         pass
     finally:
-        node.destroy_node()
         rclpy.shutdown()
 
 if __name__ == "__main__":