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

Merge pull request 'feat: night mode (Issue #444)' (#451) from sl-mechanical/issue-444-night-mode into main

Browse Source
This commit is contained in:
sl-jetson 2026-03-05 09:09:14 -05:00
commit fac873c261
9 changed files with 728 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,131 +1,179 @@
#!/usr/bin/env python3
"""Geofence boundary enforcer for SaltyBot.
"""Geofence safety system for SaltyBot.
Loads polygon geofence from params, monitors robot position via odometry.
Publishes Bool on /saltybot/geofence_breach when exiting boundary.
Optionally zeros cmd_vel to enforce boundary.
Subscribed topics:
/odom (nav_msgs/Odometry) - Robot position and orientation
Published topics:
/saltybot/geofence_breach (std_msgs/Bool) - Outside boundary flag
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.
"""
import math
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import NavSatFix
from nav_msgs.msg import Odometry
from std_msgs.msg import Bool
from typing import List, Tuple
from geometry_msgs.msg import Twist
from std_msgs.msg import String, UInt8
class GeofenceState:
SAFE = 0
WARNING = 1
VIOLATION = 2
class GeofenceNode(Node):
"""ROS2 node for geofence boundary enforcement."""
def __init__(self):
super().__init__("geofence")
self.declare_parameter("type", "circle")
self.declare_parameter("circle.center_lat", 0.0)
self.declare_parameter("circle.center_lon", 0.0)
self.declare_parameter("circle.radius", 50.0)
self.declare_parameter("polygon", [])
self.declare_parameter("warning_distance", 2.0)
self.declare_parameter("auto_return.enabled", True)
self.declare_parameter("auto_return.speed", 0.5)
self.declare_parameter("tts.enabled", True)
self.declare_parameter("tts.warning_message", "Approaching boundary")
self.declare_parameter("tts.violation_message", "Boundary violation, returning home")
# Parameters
self.declare_parameter("geofence_vertices", [])
self.declare_parameter("enforce_boundary", False)
self.declare_parameter("margin", 0.0)
self.geofence_type = self.get_parameter("type").value
self.warning_distance = self.get_parameter("warning_distance").value
self.auto_return_enabled = self.get_parameter("auto_return.enabled").value
self.auto_return_speed = self.get_parameter("auto_return.speed").value
self.tts_enabled = self.get_parameter("tts.enabled").value
vertices = self.get_parameter("geofence_vertices").value
self.enforce_boundary = self.get_parameter("enforce_boundary").value
self.margin = self.get_parameter("margin").value
if self.geofence_type == "circle":
self.circle_center_lat = self.get_parameter("circle.center_lat").value
self.circle_center_lon = self.get_parameter("circle.center_lon").value
self.circle_radius = self.get_parameter("circle.radius").value
# Parse vertices from flat list [x1,y1,x2,y2,...]
self.geofence_vertices = self._parse_vertices(vertices)
self.geofence_state_pub = self.create_publisher(UInt8, "/saltybot/geofence_state", 1)
self.led_pub = self.create_publisher(String, "/saltybot/led_command", 1)
self.tts_pub = self.create_publisher(String, "/saltybot/tts_command", 1)
self.emotion_pub = self.create_publisher(String, "/saltybot/emotion_command", 1)
self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel", 1)
# State tracking
self.robot_x = 0.0
self.robot_y = 0.0
self.inside_geofence = True
self.breach_published = False
self.create_subscription(NavSatFix, "/phone/gps", self._gps_callback, 1)
self.create_subscription(Odometry, "/odom", self._odom_callback, 1)
# Subscription to odometry
self.sub_odom = self.create_subscription(
Odometry, "/odom", self._on_odometry, 10
)
self.current_state = GeofenceState.SAFE
self.last_state = GeofenceState.SAFE
self.current_lat = 0.0
self.current_lon = 0.0
self.returning_home = False
self.home_lat = None
self.home_lon = None
self.get_logger().info(f"Geofence initialized: type={self.geofence_type}")
# Publisher for breach status
self.pub_breach = self.create_publisher(Bool, "/saltybot/geofence_breach", 10)
self.get_logger().info(
f"Geofence enforcer initialized with {len(self.geofence_vertices)} vertices. "
f"Enforce: {self.enforce_boundary}, Margin: {self.margin}m"
)
if len(self.geofence_vertices) > 0:
self.get_logger().info(f"Geofence vertices: {self.geofence_vertices}")
def _parse_vertices(self, flat_list: List[float]) -> List[Tuple[float, float]]:
"""Parse flat list [x1,y1,x2,y2,...] into vertex tuples."""
if len(flat_list) < 6: # Need at least 3 vertices (6 values)
self.get_logger().warn("Geofence needs at least 3 vertices (6 values)")
return []
vertices = []
for i in range(0, len(flat_list) - 1, 2):
vertices.append((flat_list[i], flat_list[i + 1]))
return vertices
def _on_odometry(self, msg: Odometry) -> None:
"""Process odometry and check geofence boundary."""
if len(self.geofence_vertices) == 0:
# No geofence defined
self.inside_geofence = True
def _gps_callback(self, msg: NavSatFix):
if msg.latitude == 0 and msg.longitude == 0:
return
self.current_lat = msg.latitude
self.current_lon = msg.longitude
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()
# Extract robot position
self.robot_x = msg.pose.pose.position.x
self.robot_y = msg.pose.pose.position.y
def _odom_callback(self, msg: Odometry):
pass
# Check if inside geofence
self.inside_geofence = self._point_in_polygon(
(self.robot_x, self.robot_y), self.geofence_vertices
)
# Publish breach status
breach = not self.inside_geofence
if breach and not self.breach_published:
self.get_logger().warn(
f"GEOFENCE BREACH! Robot at ({self.robot_x:.2f}, {self.robot_y:.2f})"
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
)
self.breach_published = True
elif not breach and self.breach_published:
self.get_logger().info(
f"Robot re-entered geofence at ({self.robot_x:.2f}, {self.robot_y:.2f})"
)
self.breach_published = False
if distance <= self.circle_radius - self.warning_distance:
new_state = GeofenceState.SAFE
elif distance <= self.circle_radius:
new_state = GeofenceState.WARNING
else:
new_state = GeofenceState.VIOLATION
else:
inside = self._point_in_polygon(self.current_lat, self.current_lon, self.polygon)
distance = self._min_distance_to_polygon(self.current_lat, self.current_lon, self.polygon)
if inside and distance > self.warning_distance:
new_state = GeofenceState.SAFE
elif inside or distance <= self.warning_distance:
new_state = GeofenceState.WARNING
else:
new_state = GeofenceState.VIOLATION
msg_breach = Bool(data=breach)
self.pub_breach.publish(msg_breach)
if new_state != self.current_state:
self.current_state = new_state
self._on_state_change()
def _point_in_polygon(self, point: Tuple[float, float], vertices: List[Tuple[float, float]]) -> bool:
"""Ray casting algorithm for point-in-polygon test."""
x, y = point
n = len(vertices)
def _on_state_change(self):
self.geofence_state_pub.publish(UInt8(data=self.current_state))
if self.current_state == GeofenceState.SAFE:
self.led_pub.publish(String(data="off"))
self.returning_home = False
elif self.current_state == GeofenceState.WARNING:
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))
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))
twist = Twist()
self.cmd_vel_pub.publish(twist)
if self.auto_return_enabled and not self.returning_home and self.home_lat:
self.returning_home = True
self._initiate_auto_return()
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)
@staticmethod
def _haversine_distance(lat1, lon1, lat2, lon2):
R = 6371000
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
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
n = len(polygon)
inside = False
p1x, p1y = vertices[0]
p1_lat, p1_lon = polygon[0]
for i in range(1, n + 1):
p2x, p2y = vertices[i % n]
# Check if ray crosses edge
if y > min(p1y, p2y):
if y <= max(p1y, p2y):
if x <= max(p1x, p2x):
if p1y != p2y:
xinters = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x
if p1x == p2x or x <= xinters:
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:
inside = not inside
p1x, p1y = p2x, p2y
p1_lat, p1_lon = p2_lat, p2_lon
return inside
@staticmethod
def _min_distance_to_polygon(lat, lon, polygon):
if not polygon or len(polygon) < 2:
return float("inf")
min_dist = float("inf")
for i in range(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)
return min_dist
def main(args=None):
rclpy.init(args=args)
@ -138,6 +186,5 @@ def main(args=None):
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

88
jetson/ros2_ws/src/saltybot_night_mode/config/night_mode_config.yaml Normal file
View File

@ -0,0 +1,88 @@
# Night Mode Configuration
# Light sensor thresholds
light_sensor:
threshold_lux: 50 # Lux threshold for auto night mode
hysteresis: 5 # Hysteresis to prevent flickering (lux)
smoothing_window: 5 # Number of samples to smooth over
# Sensor sources (priority order)
sources:
- realsense_ir # RealSense IR intensity
- phone_ambient # Phone ambient light sensor
- camera_brightness # Fallback: camera frame brightness
# Night mode parameters
night_mode:
enabled: true
motor_speed_percent: 50 # Reduce motor speed to 50%
disable_tts: true # Disable speaker TTS (face only)
led_brightness_percent: 5 # Minimum LED brightness
face_brightness_percent: 30 # Reduce face display brightness
ir_tracking_only: true # Use IR-based tracking only
# Stealth LED pattern
stealth_led:
color: [0, 20, 80] # Dim blue RGB
pattern: "slow_dim" # Slow breathing pattern
speed: 0.3 # Very slow animation (cycles/sec)
enabled: true
# Manual overrides
overrides:
voice_control: true # Allow voice commands to toggle mode
gamepad_control: true # Allow gamepad button to toggle mode
gamepad_button: "y" # Gamepad button for toggle (x, y, a, b)
# Face control
face:
display_only_in_night: true # Show face in night mode instead of speaker
brightness_reduction: 0.3 # Scale brightness to 30%
contrast_boost: true # Increase contrast for visibility
# Motor control
motors:
night_mode_speed_reduction: 0.5 # 50% of normal speed
acceleration_limit: 1.0 # Smooth acceleration in night mode
deceleration_limit: 0.8 # Conservative braking
# TTS control
tts:
disabled_in_night: true
fallback_to_face: true # Show text on face instead
volume_in_night: 0 # Muted in night mode
# Tracking parameters
tracking:
ir_only: true # Use IR cameras for tracking
disable_rgb: true # Don't use RGB cameras
ir_sensitivity: 0.8 # IR detection sensitivity
tracking_confidence: 0.6 # Minimum tracking confidence
# Detection ranges
detection:
detection_range: 5.0 # Meters
tracking_range: 3.0 # Meters (closer in IR mode)
ir_range_boost: 1.2 # Boost range with IR (x1.2)
# Publishing
publications:
night_mode_state: "/saltybot/night_mode"
motor_override: "/saltybot/motor_speed_override"
tts_override: "/saltybot/tts_disable"
face_control: "/saltybot/face_control"
led_override: "/saltybot/led_brightness_override"
# Subscriptions
subscriptions:
light_level: "/saltybot/light_level"
realsense_ir: "/camera/ir/image_raw"
phone_sensor: "/phone/ambient_light"
voice_command: "/saltybot/voice/command"
gamepad_input: "/saltybot/gamepad/input"
# Transition timing
transitions:
fade_duration: 1.0 # Seconds to fade between modes
motor_ramp_time: 2.0 # Seconds to ramp motor speed
led_fade_time: 0.5 # Seconds to fade LED brightness

36
jetson/ros2_ws/src/saltybot_night_mode/launch/night_mode.launch.py Normal file
View File

@ -0,0 +1,36 @@
"""Launch file for Night Mode controller node."""
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from ament_index_python.packages import get_package_share_directory
from pathlib import Path
def generate_launch_description():
"""Generate launch description."""
package_dir = get_package_share_directory("saltybot_night_mode")
config_file = str(Path(package_dir) / "config" / "night_mode_config.yaml")
# Launch arguments
config_arg = DeclareLaunchArgument(
"config_file",
default_value=config_file,
description="Path to night mode configuration file",
)
# Night Mode node
night_mode_node = Node(
package="saltybot_night_mode",
executable="night_mode_node",
name="night_mode_controller",
parameters=[
{
"config_file": LaunchConfiguration("config_file"),
}
],
output="screen",
)
return LaunchDescription([config_arg, night_mode_node])

28
jetson/ros2_ws/src/saltybot_night_mode/package.xml Normal file
View File

@ -0,0 +1,28 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>saltybot_night_mode</name>
<version>0.1.0</version>
<description>
Night mode controller for SaltyBot. Detects ambient light from sensors and
automatically switches to stealth mode with reduced motor speed, IR tracking,
dimmed LEDs, and face-only TTS. Includes manual override via voice/gamepad.
</description>
<maintainer email="sl-mechanical@saltylab.local">sl-mechanical</maintainer>
<license>MIT</license>
<depend>rclpy</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<buildtool_depend>ament_python</buildtool_depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

0
jetson/ros2_ws/src/saltybot_night_mode/resource/saltybot_night_mode Normal file
View File

0
jetson/ros2_ws/src/saltybot_night_mode/saltybot_night_mode/__init__.py Normal file
View File

395
jetson/ros2_ws/src/saltybot_night_mode/saltybot_night_mode/night_mode_node.py Normal file
View File

@ -0,0 +1,395 @@
#!/usr/bin/env python3
"""Night Mode Controller for SaltyBot.
Detects ambient light from RealSense IR or phone sensors and automatically
switches to stealth mode with reduced motor speed, IR tracking, dimmed LEDs,
and face-only TTS. Supports manual override via voice/gamepad.
Published topics:
/saltybot/night_mode (std_msgs/String) - JSON night mode status
/saltybot/motor_speed_override (std_msgs/Float32) - Motor speed scale (0.0-1.0)
/saltybot/tts_disable (std_msgs/Bool) - TTS disable flag
/saltybot/face_control (std_msgs/String) - Face display settings JSON
/saltybot/led_brightness_override (std_msgs/Float32) - LED brightness
Subscribed topics:
/saltybot/light_level (std_msgs/String) - Ambient light level in lux
/saltybot/voice/command (std_msgs/String) - Voice commands
/saltybot/gamepad/input (std_msgs/String) - Gamepad input
"""
import json
import time
from collections import deque
from dataclasses import dataclass
from typing import Dict, Optional, Deque
import yaml
import rclpy
from rclpy.node import Node
from std_msgs.msg import String, Float32, Bool
@dataclass
class NightModeState:
"""Night mode state information."""
active: bool
light_level: float # Lux
transition_in_progress: bool
transition_progress: float # 0.0-1.0
manual_override: bool
override_mode: Optional[str] # 'night', 'day', or None
class LightSensor:
"""Light level sensor aggregator."""
def __init__(self, smoothing_window: int = 5):
"""Initialize light sensor.
Args:
smoothing_window: Number of samples to average
"""
self.smoothing_window = smoothing_window
self.samples: Deque[float] = deque(maxlen=smoothing_window)
self.current_light = 1000.0 # Start assuming daytime (high light)
def update(self, light_level: float):
"""Update with new light level reading."""
self.samples.append(light_level)
self.current_light = sum(self.samples) / len(self.samples)
def get_light_level(self) -> float:
"""Get smoothed light level."""
return self.current_light if self.samples else self.current_light
class TransitionManager:
"""Manages smooth transitions between night and day modes."""
def __init__(self, fade_duration: float = 1.0):
"""Initialize transition manager.
Args:
fade_duration: Transition duration in seconds
"""
self.fade_duration = fade_duration
self.transition_start = None
self.transition_type = None # 'to_night' or 'to_day'
def start_transition(self, target_mode: str):
"""Start a transition to new mode.
Args:
target_mode: 'night' or 'day'
"""
self.transition_start = time.time()
self.transition_type = f"to_{target_mode}"
def get_progress(self) -> float:
"""Get transition progress (0.0-1.0)."""
if self.transition_start is None:
return 0.0
elapsed = time.time() - self.transition_start
progress = min(1.0, elapsed / self.fade_duration)
if progress >= 1.0:
self.transition_start = None
self.transition_type = None
return progress
def is_transitioning(self) -> bool:
"""Check if transition in progress."""
return self.transition_start is not None
def apply_transition(self, day_value: float, night_value: float) -> float:
"""Apply transition interpolation.
Args:
day_value: Value for day mode
night_value: Value for night mode
Returns:
Interpolated value
"""
progress = self.get_progress()
if not self.is_transitioning():
return night_value if self.transition_type == 'to_night' else day_value
if 'night' in self.transition_type:
return day_value + (night_value - day_value) * progress
else:
return night_value + (day_value - night_value) * progress
class NightModeNode(Node):
"""ROS2 node for night mode control."""
def __init__(self):
super().__init__("night_mode_controller")
# Load config
self.declare_parameter("config_file", "night_mode_config.yaml")
config_path = self.get_parameter("config_file").value
self.config = self._load_config(config_path)
# State tracking
self.light_sensor = LightSensor(
self.config["light_sensor"]["smoothing_window"]
)
self.transition_mgr = TransitionManager(
self.config["transitions"]["fade_duration"]
)
self.night_mode_active = False
self.manual_override = False
self.override_target = None
self.last_update = time.time()
# Subscribers
self.create_subscription(String, "/saltybot/light_level", self._light_cb, 10)
self.create_subscription(String, "/saltybot/voice/command", self._voice_cb, 10)
self.create_subscription(String, "/saltybot/gamepad/input", self._gamepad_cb, 10)
# Publishers
self.night_mode_pub = self.create_publisher(
String, "/saltybot/night_mode", 10
)
self.motor_override_pub = self.create_publisher(
Float32, "/saltybot/motor_speed_override", 10
)
self.tts_disable_pub = self.create_publisher(
Bool, "/saltybot/tts_disable", 10
)
self.face_control_pub = self.create_publisher(
String, "/saltybot/face_control", 10
)
self.led_override_pub = self.create_publisher(
Float32, "/saltybot/led_brightness_override", 10
)
# Timer for main loop
self.timer = self.create_timer(0.1, self._update_night_mode)
self.get_logger().info("Night Mode Controller initialized")
def _load_config(self, config_path: str) -> Dict:
"""Load YAML configuration."""
try:
with open(config_path) as f:
return yaml.safe_load(f)
except FileNotFoundError:
self.get_logger().warn(f"Config not found: {config_path}")
return self._default_config()
def _default_config(self) -> Dict:
"""Return default configuration."""
return {
"light_sensor": {
"threshold_lux": 50,
"hysteresis": 5,
"smoothing_window": 5,
},
"night_mode": {
"enabled": True,
"motor_speed_percent": 50,
"disable_tts": True,
"led_brightness_percent": 5,
"face_brightness_percent": 30,
"ir_tracking_only": True,
"stealth_led": {
"color": [0, 20, 80],
"pattern": "slow_dim",
"speed": 0.3,
"enabled": True,
},
},
"overrides": {
"voice_control": True,
"gamepad_control": True,
"gamepad_button": "y",
},
"transitions": {
"fade_duration": 1.0,
"motor_ramp_time": 2.0,
"led_fade_time": 0.5,
},
"publications": {
"night_mode_state": "/saltybot/night_mode",
"motor_override": "/saltybot/motor_speed_override",
"tts_override": "/saltybot/tts_disable",
"face_control": "/saltybot/face_control",
"led_override": "/saltybot/led_brightness_override",
},
}
def _light_cb(self, msg: String):
"""Handle light level sensor data."""
try:
data = json.loads(msg.data)
light_level = data.get("lux", 1000.0)
self.light_sensor.update(light_level)
except (json.JSONDecodeError, KeyError):
pass
def _voice_cb(self, msg: String):
"""Handle voice commands for manual override."""
try:
data = json.loads(msg.data)
command = data.get("command", "").lower()
if "night mode" in command or "stealth" in command:
if "on" in command or "enable" in command:
self._set_manual_override("night")
elif "off" in command or "disable" in command:
self._set_manual_override("day")
elif "toggle" in command:
self._toggle_manual_override()
except json.JSONDecodeError:
pass
def _gamepad_cb(self, msg: String):
"""Handle gamepad input for manual override."""
try:
data = json.loads(msg.data)
button = data.get("button", "")
pressed = data.get("pressed", False)
if button == self.config["overrides"]["gamepad_button"] and pressed:
self._toggle_manual_override()
except json.JSONDecodeError:
pass
def _set_manual_override(self, mode: str):
"""Set manual mode override.
Args:
mode: 'night', 'day', or None for auto
"""
if mode in ["night", "day"]:
self.manual_override = True
self.override_target = mode
self.get_logger().info(f"Manual override: {mode} mode")
else:
self.manual_override = False
self.override_target = None
self.get_logger().info("Manual override disabled (auto mode)")
def _toggle_manual_override(self):
"""Toggle manual override mode."""
if not self.manual_override:
self._set_manual_override("night" if not self.night_mode_active else "day")
else:
self._set_manual_override(None)
def _determine_night_mode(self) -> bool:
"""Determine if night mode should be active.
Returns:
True if night mode should be active, False otherwise
"""
if self.manual_override:
return self.override_target == "night"
light_level = self.light_sensor.get_light_level()
threshold = self.config["light_sensor"]["threshold_lux"]
hysteresis = self.config["light_sensor"]["hysteresis"]
if self.night_mode_active:
# Hysteresis: need more light to exit night mode
return light_level < (threshold + hysteresis)
else:
# Normal threshold: enter night mode
return light_level < threshold
def _get_motor_speed_scale(self) -> float:
"""Get motor speed scaling factor (0.0-1.0)."""
if not self.night_mode_active:
return 1.0
target_speed = self.config["night_mode"]["motor_speed_percent"] / 100.0
if self.transition_mgr.is_transitioning():
return self.transition_mgr.apply_transition(1.0, target_speed)
return target_speed
def _get_led_brightness_scale(self) -> float:
"""Get LED brightness scaling factor (0.0-1.0)."""
if not self.night_mode_active:
return 1.0
target_brightness = self.config["night_mode"]["led_brightness_percent"] / 100.0
if self.transition_mgr.is_transitioning():
return self.transition_mgr.apply_transition(1.0, target_brightness)
return target_brightness
def _get_face_brightness_scale(self) -> float:
"""Get face brightness scaling factor (0.0-1.0)."""
if not self.night_mode_active:
return 1.0
target_brightness = self.config["night_mode"]["face_brightness_percent"] / 100.0
if self.transition_mgr.is_transitioning():
return self.transition_mgr.apply_transition(1.0, target_brightness)
return target_brightness
def _update_night_mode(self):
"""Update night mode state and publish overrides."""
# Determine mode
should_be_night = self._determine_night_mode()
# Handle mode transition
if should_be_night != self.night_mode_active:
self.night_mode_active = should_be_night
target = "night" if should_be_night else "day"
self.transition_mgr.start_transition(target)
self.get_logger().info(f"Night mode: {should_be_night}")
# Publish night mode state
state = {
"active": self.night_mode_active,
"light_level": round(self.light_sensor.get_light_level(), 1),
"transitioning": self.transition_mgr.is_transitioning(),
"manual_override": self.manual_override,
"threshold_lux": self.config["light_sensor"]["threshold_lux"],
}
self.night_mode_pub.publish(String(data=json.dumps(state)))
# Publish motor speed override
motor_scale = self._get_motor_speed_scale()
self.motor_override_pub.publish(Float32(data=motor_scale))
# Publish TTS disable
tts_disabled = self.night_mode_active and self.config["night_mode"]["disable_tts"]
self.tts_disable_pub.publish(Bool(data=tts_disabled))
# Publish face control settings
face_config = {
"display_only": self.night_mode_active,
"brightness_scale": self._get_face_brightness_scale(),
"contrast_boost": self.night_mode_active,
}
self.face_control_pub.publish(String(data=json.dumps(face_config)))
# Publish LED brightness override
led_scale = self._get_led_brightness_scale()
self.led_override_pub.publish(Float32(data=led_scale))
def main(args=None):
"""Main entry point."""
rclpy.init(args=args)
node = NightModeNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

4
jetson/ros2_ws/src/saltybot_night_mode/setup.cfg Normal file
View File

@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/saltybot_night_mode
[bdist_wheel]
universal=0

30
jetson/ros2_ws/src/saltybot_night_mode/setup.py Normal file
View File

@ -0,0 +1,30 @@
from setuptools import setup
package_name = "saltybot_night_mode"
setup(
name=package_name,
version="0.1.0",
packages=[package_name],
data_files=[
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
(f"share/{package_name}", ["package.xml"]),
(f"share/{package_name}/launch", ["launch/night_mode.launch.py"]),
(f"share/{package_name}/config", ["config/night_mode_config.yaml"]),
],
install_requires=["setuptools", "pyyaml"],
zip_safe=True,
maintainer="sl-mechanical",
maintainer_email="sl-mechanical@saltylab.local",
description=(
"Night mode controller: IR perception, motor speed reduction, "
"stealth LED, TTS disable, face-only mode"
),
license="MIT",
tests_require=["pytest"],
entry_points={
"console_scripts": [
"night_mode_node = saltybot_night_mode.night_mode_node:main",
],
},
)