sl-jetson
ea26cda76a
Add battery_node.py: - Subscribes /saltybot/telemetry/battery (JSON from stm32_cmd_node) - Publishes sensor_msgs/BatteryState on /saltybot/battery at 1 Hz - SoC source priority: STM32 fuel gauge soc_pct field → fallback to 3S LiPo voltage curve (12-point lookup with linear interpolation) - Charging detection: current_ma < -100 mA threshold - Alert levels: WARNING (20%)→speed 60%, CRITICAL (10%)→speed 30%, EMERGENCY (5%)→zero /cmd_vel + /saltybot/arm(False) disarm - /saltybot/battery/alert JSON topic on threshold crossings - /saltybot/speed_limit Float32 (0.0-1.0) for nav speed capping - SQLite history logging: /var/log/saltybot/battery.db, 7-day retention - Hourly prune timer to keep DB bounded Add test_battery.py (70+ tests, no ROS2 runtime): - SoC lookup: all curve points, interpolation, clamping, 3S/4S packs - Alert level thresholds and transitions for all levels - Speed factor assignments per alert level - Charging detection logic - sensor_msgs/BatteryState field population - SQLite insert/retrieve/prune (in-memory and on-disk) - JSON telemetry parsing: normal, charging, soc_pct=0 fallback Add battery_params.yaml, battery.launch.py. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
60 lines
2.6 KiB
Python
60 lines
2.6 KiB
Python
"""battery.launch.py — Launch the battery management node (Issue #125).
|
|
|
|
Usage:
|
|
ros2 launch saltybot_bridge battery.launch.py
|
|
|
|
# Custom thresholds:
|
|
ros2 launch saltybot_bridge battery.launch.py \
|
|
warn_pct:=25.0 critical_pct:=12.0 emergency_pct:=6.0
|
|
|
|
# Custom DB path:
|
|
ros2 launch saltybot_bridge battery.launch.py \
|
|
db_path:=/mnt/nvme/saltybot/battery.db
|
|
"""
|
|
|
|
import os
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
|
|
|
|
def generate_launch_description() -> LaunchDescription:
|
|
pkg = get_package_share_directory("saltybot_bridge")
|
|
params_file = os.path.join(pkg, "config", "battery_params.yaml")
|
|
|
|
return LaunchDescription([
|
|
DeclareLaunchArgument("db_path", default_value="/var/log/saltybot/battery.db"),
|
|
DeclareLaunchArgument("cell_count", default_value="3"),
|
|
DeclareLaunchArgument("warn_pct", default_value="20.0"),
|
|
DeclareLaunchArgument("critical_pct", default_value="10.0"),
|
|
DeclareLaunchArgument("emergency_pct", default_value="5.0"),
|
|
DeclareLaunchArgument("warn_speed_factor", default_value="0.6"),
|
|
DeclareLaunchArgument("critical_speed_factor", default_value="0.3"),
|
|
DeclareLaunchArgument("charge_detect_ma", default_value="-100"),
|
|
DeclareLaunchArgument("publish_rate_hz", default_value="1.0"),
|
|
|
|
Node(
|
|
package="saltybot_bridge",
|
|
executable="battery_node",
|
|
name="battery_node",
|
|
output="screen",
|
|
emulate_tty=True,
|
|
parameters=[
|
|
params_file,
|
|
{
|
|
"db_path": LaunchConfiguration("db_path"),
|
|
"cell_count": LaunchConfiguration("cell_count"),
|
|
"warn_pct": LaunchConfiguration("warn_pct"),
|
|
"critical_pct": LaunchConfiguration("critical_pct"),
|
|
"emergency_pct": LaunchConfiguration("emergency_pct"),
|
|
"warn_speed_factor": LaunchConfiguration("warn_speed_factor"),
|
|
"critical_speed_factor": LaunchConfiguration("critical_speed_factor"),
|
|
"charge_detect_ma": LaunchConfiguration("charge_detect_ma"),
|
|
"publish_rate_hz": LaunchConfiguration("publish_rate_hz"),
|
|
},
|
|
],
|
|
),
|
|
])
|