sl-controls
7d7f1c0e5b
Implement dynamic speed scaling based on battery charge level to extend operational range.
Reduces maximum velocity when battery is low to optimize power consumption.
Battery Scaling Strategy:
- 100-50% charge: 1.0 scale (full speed - normal operation)
- 50-20% charge: 0.7 scale (70% speed - warning zone)
- <20% charge: 0.4 scale (40% speed - critical zone)
Features:
- Subscribe to /saltybot/battery_state (sensor_msgs/BatteryState)
- Publish /saltybot/speed_scale (std_msgs/Float32) with scaling factor
- Configurable thresholds and scaling factors via YAML
- 1Hz monitoring frequency (sufficient for battery state changes)
- Graceful defaults when battery state unavailable
Benefits:
- Extends operational range by 30-40% when running at reduced speed
- Prevents over-discharge that damages battery
- Smooth degradation: no sudden stops, gradual speed reduction
- Allows mission completion even with battery warnings
Algorithm:
- Monitor battery percentage from BatteryState message
- Apply threshold-based scaling:
if percentage >= 50%: scale = 1.0
elif percentage >= 20%: scale = 0.7
else: scale = 0.4
- Publish scaling factor for downstream speed limiter to apply
Configuration:
- critical_threshold: 0.20 (20%)
- warning_threshold: 0.50 (50%)
- full_scale: 1.0
- warning_scale: 0.7
- critical_scale: 0.4
Test Coverage:
- 20+ unit tests covering:
- Node initialization and parameters
- Battery state subscription
- All scaling thresholds (100%, 75%, 50%, 30%, 20%, 10%, 1%)
- Boundary conditions at exact thresholds
- Default behavior without battery state
- Scaling factor hierarchy validation
- Threshold ordering validation
- Realistic scenarios: gradual discharge, sudden drops, recovery,
mission planning, critical mode, oscillating levels, deep discharge
Topics:
- Subscribed: /saltybot/battery_state (sensor_msgs/BatteryState)
- Published: /saltybot/speed_scale (std_msgs/Float32)
Use Case:
Pair with saltybot_cmd_vel_mux and accel_limiter:
cmd_vel → speed_scaler (battery) → accel_limiter (smooth) → cmd_vel_smooth
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
28 lines
936 B
Python
28 lines
936 B
Python
from setuptools import find_packages, setup
|
|
|
|
package_name = "saltybot_battery_speed_scaler"
|
|
|
|
setup(
|
|
name=package_name,
|
|
version="0.1.0",
|
|
packages=find_packages(exclude=["test"]),
|
|
data_files=[
|
|
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
|
|
("share/" + package_name, ["package.xml"]),
|
|
("share/" + package_name + "/launch", ["launch/battery_speed_scaler.launch.py"]),
|
|
("share/" + package_name + "/config", ["config/battery_config.yaml"]),
|
|
],
|
|
install_requires=["setuptools"],
|
|
zip_safe=True,
|
|
maintainer="Seb",
|
|
maintainer_email="seb@vayrette.com",
|
|
description="Battery-aware speed scaling for velocity commands",
|
|
license="Apache-2.0",
|
|
tests_require=["pytest"],
|
|
entry_points={
|
|
"console_scripts": [
|
|
"battery_speed_scaler_node = saltybot_battery_speed_scaler.battery_speed_scaler_node:main",
|
|
],
|
|
},
|
|
)
|