jetson/ros2_ws/src/saltybot_battery_speed_scaler/config/battery_config.yaml

@@ -1,17 +0,0 @@
-# Battery-aware speed scaling configuration
-
-battery_speed_scaler:
-  ros__parameters:
-    # Update frequency (Hz)
-    frequency: 1  # 1 Hz is sufficient for battery monitoring
-
-    # Battery level thresholds (0.0 to 1.0 percentage)
-    # Below these thresholds, speed is reduced
-    critical_threshold: 0.20  # 20% - critical battery
-    warning_threshold: 0.50   # 50% - moderate discharge
-
-    # Speed scaling factors (0.0 to 1.0)
-    # Applied to max velocity when battery is below thresholds
-    full_scale: 1.0    # >= 50% battery: full speed
-    warning_scale: 0.7 # 20-50% battery: 70% speed
-    critical_scale: 0.4 # < 20% battery: 40% speed

jetson/ros2_ws/src/saltybot_battery_speed_scaler/launch/battery_speed_scaler.launch.py

@@ -1,36 +0,0 @@
-"""Launch file for battery_speed_scaler_node."""
-
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument
-import os
-from ament_index_python.packages import get_package_share_directory
-
-
-def generate_launch_description():
-    """Generate launch description for battery speed scaler node."""
-    # Package directory
-    pkg_dir = get_package_share_directory("saltybot_battery_speed_scaler")
-
-    # Parameters
-    config_file = os.path.join(pkg_dir, "config", "battery_config.yaml")
-
-    # Declare launch arguments
-    return LaunchDescription(
-        [
-            DeclareLaunchArgument(
-                "config_file",
-                default_value=config_file,
-                description="Path to configuration YAML file",
-            ),
-            # Battery speed scaler node
-            Node(
-                package="saltybot_battery_speed_scaler",
-                executable="battery_speed_scaler_node",
-                name="battery_speed_scaler",
-                output="screen",
-                parameters=[LaunchConfiguration("config_file")],
-            ),
-        ]
-    )

jetson/ros2_ws/src/saltybot_battery_speed_scaler/package.xml

@@ -1,21 +0,0 @@
-<?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_battery_speed_scaler</name>
-  <version>0.1.0</version>
-  <description>Battery-aware speed scaling for SaltyBot.</description>
-  <maintainer email="seb@vayrette.com">Seb</maintainer>
-  <license>Apache-2.0</license>
-
-  <buildtool_depend>ament_python</buildtool_depend>
-
-  <depend>rclpy</depend>
-  <depend>sensor_msgs</depend>
-  <depend>std_msgs</depend>
-
-  <test_depend>pytest</test_depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>

jetson/ros2_ws/src/saltybot_battery_speed_scaler/saltybot_battery_speed_scaler/battery_speed_scaler_node.py

@@ -1,119 +0,0 @@
-#!/usr/bin/env python3
-"""Battery-aware speed scaling for SaltyBot.
-
-Subscribes to battery state and scales maximum velocity based on battery level.
-Prevents over-discharge and extends operational range.
-
-Subscribed topics:
-  /saltybot/battery_state (sensor_msgs/BatteryState) - Battery status
-
-Published topics:
-  /saltybot/speed_scale (std_msgs/Float32) - Speed scaling factor (0.0-1.0)
-
-Battery level thresholds:
-  100-50%:  1.0 scale (full speed)
-  50-20%:   0.7 scale (70% speed)
-  <20%:     0.4 scale (40% speed - critical)
-"""
-
-from typing import Optional
-
-import rclpy
-from rclpy.node import Node
-from rclpy.timer import Timer
-from sensor_msgs.msg import BatteryState
-from std_msgs.msg import Float32
-
-
-class BatterySpeedScalerNode(Node):
-    """ROS2 node for battery-aware speed scaling."""
-
-    def __init__(self):
-        super().__init__("battery_speed_scaler")
-
-        # Parameters
-        self.declare_parameter("frequency", 1)  # Hz
-        frequency = self.get_parameter("frequency").value
-
-        # Battery thresholds (percentage)
-        self.declare_parameter("critical_threshold", 20.0)
-        self.declare_parameter("warning_threshold", 50.0)
-
-        # Speed scaling factors
-        self.declare_parameter("full_scale", 1.0)
-        self.declare_parameter("warning_scale", 0.7)
-        self.declare_parameter("critical_scale", 0.4)
-
-        self.critical_threshold = self.get_parameter("critical_threshold").value
-        self.warning_threshold = self.get_parameter("warning_threshold").value
-        self.full_scale = self.get_parameter("full_scale").value
-        self.warning_scale = self.get_parameter("warning_scale").value
-        self.critical_scale = self.get_parameter("critical_scale").value
-
-        # Latest battery state
-        self.battery_state: Optional[BatteryState] = None
-
-        # Subscription
-        self.create_subscription(
-            BatteryState, "/saltybot/battery_state", self._on_battery_state, 10
-        )
-
-        # Publisher for speed scale
-        self.pub_scale = self.create_publisher(Float32, "/saltybot/speed_scale", 10)
-
-        # Timer for speed scaling at configured frequency
-        period = 1.0 / frequency
-        self.timer: Timer = self.create_timer(period, self._timer_callback)
-
-        self.get_logger().info(
-            f"Battery speed scaler initialized at {frequency}Hz. "
-            f"Thresholds: warning={self.warning_threshold}%, "
-            f"critical={self.critical_threshold}%. "
-            f"Scale factors: full={self.full_scale}, "
-            f"warning={self.warning_scale}, critical={self.critical_scale}"
-        )
-
-    def _on_battery_state(self, msg: BatteryState) -> None:
-        """Update battery state from subscription."""
-        self.battery_state = msg
-
-    def _timer_callback(self) -> None:
-        """Compute and publish speed scale based on battery level."""
-        if self.battery_state is None:
-            # No battery state received yet, default to full speed
-            scale = self.full_scale
-        else:
-            # Convert battery percentage to 0-100 scale
-            battery_percent = self.battery_state.percentage * 100.0
-
-            # Determine speed scale based on battery level
-            if battery_percent >= self.warning_threshold:
-                # Good battery level: full speed
-                scale = self.full_scale
-            elif battery_percent >= self.critical_threshold:
-                # Moderate discharge: warning speed
-                scale = self.warning_scale
-            else:
-                # Critical battery: reduced speed
-                scale = self.critical_scale
-
-        # Publish speed scale
-        scale_msg = Float32()
-        scale_msg.data = scale
-        self.pub_scale.publish(scale_msg)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = BatterySpeedScalerNode()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

jetson/ros2_ws/src/saltybot_battery_speed_scaler/setup.cfg

@@ -1,4 +0,0 @@
-[develop]
-script-dir=$base/lib/saltybot_battery_speed_scaler
-[install]
-install-scripts=$base/lib/saltybot_battery_speed_scaler

jetson/ros2_ws/src/saltybot_battery_speed_scaler/setup.py

@@ -1,27 +0,0 @@
-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",
-        ],
-    },
-)

jetson/ros2_ws/src/saltybot_battery_speed_scaler/test/test_battery_speed_scaler.py

@@ -1,400 +0,0 @@
-"""Unit tests for battery_speed_scaler_node."""
-
-import pytest
-from sensor_msgs.msg import BatteryState
-from std_msgs.msg import Float32
-
-import rclpy
-
-# Import the node under test
-from saltybot_battery_speed_scaler.battery_speed_scaler_node import BatterySpeedScalerNode
-
-
-@pytest.fixture
-def rclpy_fixture():
-    """Initialize and cleanup rclpy."""
-    rclpy.init()
-    yield
-    rclpy.shutdown()
-
-
-@pytest.fixture
-def node(rclpy_fixture):
-    """Create a battery speed scaler node instance."""
-    node = BatterySpeedScalerNode()
-    yield node
-    node.destroy_node()
-
-
-class TestNodeInitialization:
-    """Test suite for node initialization."""
-
-    def test_node_initialization(self, node):
-        """Test that node initializes with correct defaults."""
-        assert node.battery_state is None
-        assert node.critical_threshold == 0.20
-        assert node.warning_threshold == 0.50
-        assert node.full_scale == 1.0
-        assert node.warning_scale == 0.7
-        assert node.critical_scale == 0.4
-
-    def test_frequency_parameter(self, node):
-        """Test frequency parameter is set correctly."""
-        frequency = node.get_parameter("frequency").value
-        assert frequency == 1
-
-    def test_threshold_parameters(self, node):
-        """Test threshold parameters are set correctly."""
-        critical = node.get_parameter("critical_threshold").value
-        warning = node.get_parameter("warning_threshold").value
-        assert critical == 0.20
-        assert warning == 0.50
-
-    def test_scale_parameters(self, node):
-        """Test scale factor parameters are set correctly."""
-        full = node.get_parameter("full_scale").value
-        warning = node.get_parameter("warning_scale").value
-        critical = node.get_parameter("critical_scale").value
-        assert full == 1.0
-        assert warning == 0.7
-        assert critical == 0.4
-
-
-class TestBatteryStateSubscription:
-    """Test suite for battery state subscription."""
-
-    def test_battery_state_subscription(self, node):
-        """Test that battery state subscription updates node state."""
-        battery = BatteryState()
-        battery.percentage = 0.75  # 75%
-
-        node._on_battery_state(battery)
-
-        assert node.battery_state is battery
-        assert node.battery_state.percentage == 0.75
-
-    def test_multiple_battery_updates(self, node):
-        """Test that subscription updates replace previous state."""
-        battery1 = BatteryState()
-        battery1.percentage = 0.75
-
-        battery2 = BatteryState()
-        battery2.percentage = 0.50
-
-        node._on_battery_state(battery1)
-        assert node.battery_state.percentage == 0.75
-
-        node._on_battery_state(battery2)
-        assert node.battery_state.percentage == 0.50
-
-
-class TestSpeedScaling:
-    """Test suite for speed scaling logic."""
-
-    def test_full_battery_full_speed(self, node):
-        """Test full speed at high battery level."""
-        battery = BatteryState()
-        battery.percentage = 1.0  # 100%
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish full scale
-        assert True  # Timer callback executes without error
-
-    def test_high_battery_full_speed(self, node):
-        """Test full speed at 75% battery."""
-        battery = BatteryState()
-        battery.percentage = 0.75  # 75%
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish full scale
-        assert True
-
-    def test_threshold_battery_full_speed(self, node):
-        """Test full speed at warning threshold (50%)."""
-        battery = BatteryState()
-        battery.percentage = 0.50  # 50% - at warning threshold
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish full scale (>= warning threshold)
-        assert True
-
-    def test_above_warning_threshold_full_speed(self, node):
-        """Test full speed at 51% (just above warning threshold)."""
-        battery = BatteryState()
-        battery.percentage = 0.51  # 51%
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish full scale
-        assert True
-
-    def test_below_warning_threshold_warning_scale(self, node):
-        """Test warning scale at 49% (just below warning threshold)."""
-        battery = BatteryState()
-        battery.percentage = 0.49  # 49%
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish warning scale
-        assert True
-
-    def test_warning_battery_warning_scale(self, node):
-        """Test warning scale at 30% battery."""
-        battery = BatteryState()
-        battery.percentage = 0.30  # 30%
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish warning scale
-        assert True
-
-    def test_critical_threshold_warning_scale(self, node):
-        """Test warning scale at critical threshold (20%)."""
-        battery = BatteryState()
-        battery.percentage = 0.20  # 20% - at critical threshold
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish warning scale (>= critical threshold)
-        assert True
-
-    def test_above_critical_threshold_warning_scale(self, node):
-        """Test warning scale at 21% (just above critical threshold)."""
-        battery = BatteryState()
-        battery.percentage = 0.21  # 21%
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish warning scale
-        assert True
-
-    def test_below_critical_threshold_critical_scale(self, node):
-        """Test critical scale at 19% (just below critical threshold)."""
-        battery = BatteryState()
-        battery.percentage = 0.19  # 19%
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish critical scale
-        assert True
-
-    def test_critical_battery_critical_scale(self, node):
-        """Test critical scale at 10% battery."""
-        battery = BatteryState()
-        battery.percentage = 0.10  # 10%
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish critical scale
-        assert True
-
-    def test_empty_battery_critical_scale(self, node):
-        """Test critical scale at 1% battery."""
-        battery = BatteryState()
-        battery.percentage = 0.01  # 1%
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should publish critical scale
-        assert True
-
-    def test_no_battery_state_defaults_to_full(self, node):
-        """Test that node defaults to full speed without battery state."""
-        node.battery_state = None
-        node._timer_callback()
-
-        # Should publish full scale as default
-        assert True
-
-
-class TestScalingBoundaries:
-    """Test suite for scaling factor boundaries."""
-
-    def test_scaling_factors_valid_range(self, node):
-        """Test that scaling factors are within valid range."""
-        assert 0.0 <= node.full_scale <= 1.0
-        assert 0.0 <= node.warning_scale <= 1.0
-        assert 0.0 <= node.critical_scale <= 1.0
-
-    def test_scaling_hierarchy(self, node):
-        """Test that scaling factors follow proper hierarchy."""
-        # Critical should be most restrictive
-        assert node.critical_scale <= node.warning_scale
-        assert node.warning_scale <= node.full_scale
-
-    def test_threshold_order(self, node):
-        """Test that thresholds are in proper order."""
-        assert node.critical_threshold < node.warning_threshold
-
-    def test_custom_scaling_factors(self, rclpy_fixture):
-        """Test node with custom scaling factors."""
-        rclpy.init()
-        node = BatterySpeedScalerNode()
-
-        # Thresholds are configurable
-        assert node.critical_threshold == 0.20
-        assert node.warning_threshold == 0.50
-
-        node.destroy_node()
-
-
-class TestScenarios:
-    """Integration-style tests for realistic scenarios."""
-
-    def test_scenario_full_charge_operation(self, node):
-        """Scenario: Robot starts with full charge."""
-        battery = BatteryState()
-        battery.percentage = 1.0
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should operate at full speed
-        assert True
-
-    def test_scenario_gradual_discharge(self, node):
-        """Scenario: Battery gradually discharges during operation."""
-        discharge_levels = [1.0, 0.75, 0.55, 0.50, 0.40, 0.20, 0.10, 0.05]
-
-        for level in discharge_levels:
-            battery = BatteryState()
-            battery.percentage = level
-
-            node._on_battery_state(battery)
-            node._timer_callback()
-
-        # Should handle all discharge levels
-        assert True
-
-    def test_scenario_sudden_power_loss(self, node):
-        """Scenario: Battery suddenly drops due to power surge."""
-        # High battery
-        battery1 = BatteryState()
-        battery1.percentage = 0.80
-        node._on_battery_state(battery1)
-        node._timer_callback()
-
-        # Sudden drop to critical
-        battery2 = BatteryState()
-        battery2.percentage = 0.15
-        node._on_battery_state(battery2)
-        node._timer_callback()
-
-        # Should gracefully handle jump to critical
-        assert True
-
-    def test_scenario_battery_recovery(self, node):
-        """Scenario: Battery level recovers (perhaps after rest)."""
-        # Start critical
-        battery1 = BatteryState()
-        battery1.percentage = 0.10
-        node._on_battery_state(battery1)
-        node._timer_callback()
-
-        # Recovery
-        battery2 = BatteryState()
-        battery2.percentage = 0.60
-        node._on_battery_state(battery2)
-        node._timer_callback()
-
-        # Should adapt to recovered battery level
-        assert True
-
-    def test_scenario_mission_completion_before_critical(self, node):
-        """Scenario: Operator manages speed based on battery warnings."""
-        battery_levels = [0.90, 0.60, 0.52, 0.50, 0.45, 0.25, 0.22, 0.20]
-
-        for level in battery_levels:
-            battery = BatteryState()
-            battery.percentage = level
-
-            node._on_battery_state(battery)
-            node._timer_callback()
-
-            # At 50% crosses into warning zone, should reduce speed
-            # At 20% crosses into critical, should reduce further
-            assert True
-
-    def test_scenario_emergency_low_battery_return(self, node):
-        """Scenario: Robot enters critical mode and must return home."""
-        # Already low battery when emergency triggers
-        battery = BatteryState()
-        battery.percentage = 0.15
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Should limit to critical scale (40%) to extend range
-        assert True
-
-    def test_scenario_constant_monitoring(self, node):
-        """Scenario: Continuous battery monitoring during operation."""
-        # Simulate 100 time steps with varying battery
-        for i in range(100):
-            battery = BatteryState()
-            # Gradual discharge: 100% down to 0%
-            battery.percentage = 1.0 - (i / 100.0)
-
-            node._on_battery_state(battery)
-            node._timer_callback()
-
-        # Should handle continuous monitoring
-        assert True
-
-    def test_scenario_hysteresis_needed(self, node):
-        """Scenario: Battery level oscillates near threshold."""
-        # Oscillate near 50% threshold
-        thresholds_crossing = [0.51, 0.49, 0.51, 0.49, 0.51, 0.49]
-
-        for level in thresholds_crossing:
-            battery = BatteryState()
-            battery.percentage = level
-
-            node._on_battery_state(battery)
-            node._timer_callback()
-
-        # Should handle oscillations (without hysteresis, may cause
-        # rapid scale changes. This is acceptable for this node.)
-        assert True
-
-    def test_scenario_deep_discharge_protection(self, node):
-        """Scenario: Approaching minimum safe voltage."""
-        critical_levels = [0.20, 0.15, 0.10, 0.05, 0.01]
-
-        for level in critical_levels:
-            battery = BatteryState()
-            battery.percentage = level
-
-            node._on_battery_state(battery)
-            node._timer_callback()
-
-            # All below critical should use critical scale
-            assert True
-
-    def test_scenario_cold_weather_reduced_capacity(self, node):
-        """Scenario: Cold weather reduces effective battery capacity."""
-        # Battery reports 60% but effectively lower due to temperature
-        battery = BatteryState()
-        battery.percentage = 0.60
-        battery.temperature = 273 + (-10)  # -10°C
-
-        node._on_battery_state(battery)
-        node._timer_callback()
-
-        # Node should publish based on reported percentage (60% = full scale)
-        # Temperature compensation would be separate concern
-        assert True