seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 5 Actions Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'origin/sl-jetson/issue-447-full-launch'

Browse Source 
# Conflicts:
#	jetson/ros2_ws/src/saltybot_geofence/saltybot_geofence/geofence_node.py
This commit is contained in:
sl-jetson 2026-03-05 09:10:33 -05:00
commit 3ea19fbb99
3 changed files with 332 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

181
jetson/ros2_ws/src/saltybot_bringup/config/saltybot_params.yaml Normal file
View File

@ -0,0 +1,181 @@
# 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

116
jetson/ros2_ws/src/saltybot_bringup/saltybot_bringup/stack_state_monitor_node.py Normal file
View File

@ -0,0 +1,116 @@
"""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()

44
jetson/ros2_ws/src/saltybot_geofence/saltybot_geofence/geofence_node.py
View File

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