From 3c438595e804c64f6fa58247677ec5351970c207 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Mon, 2 Mar 2026 08:45:12 -0500 Subject: [PATCH] feat(rover): SaltyRover 4-wheel ESC motor driver (Issue #110) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - kinematics.py: pure unicycle→differential/skid-steer kinematics, speed_to_pwm (1000–2000µs), compute_wheel_speeds with ±max clip, odometry_from_wheel_speeds inverse helper - rover_driver_node.py: 50 Hz ROS2 node; serial P,,,\n protocol; heartbeat H\n; deadman on /cmd_vel silence; runtime 2WD/4WD variant switch via four_wheel param; dead-reckoning odometry; publishes /saltybot/rover_pwm (JSON) + /saltybot/rover_odom - config/rover_params.yaml, launch/rover_driver.launch.py, package.xml, setup.py, setup.cfg - test/test_rover_kinematics.py: 51 unit tests, all passing Co-Authored-By: Claude Sonnet 4.6 --- .../config/rover_params.yaml | 26 ++ .../launch/rover_driver.launch.py | 53 +++ .../src/saltybot_rover_driver/package.xml | 23 ++ .../resource/saltybot_rover_driver | 0 .../saltybot_rover_driver/__init__.py | 0 .../saltybot_rover_driver/kinematics.py | 156 +++++++++ .../rover_driver_node.py | 326 ++++++++++++++++++ .../src/saltybot_rover_driver/setup.cfg | 5 + .../src/saltybot_rover_driver/setup.py | 32 ++ .../test/test_rover_kinematics.py | 295 ++++++++++++++++ 10 files changed, 916 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_rover_driver/config/rover_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_rover_driver/launch/rover_driver.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_rover_driver/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_rover_driver/resource/saltybot_rover_driver create mode 100644 jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/kinematics.py create mode 100644 jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/rover_driver_node.py create mode 100644 jetson/ros2_ws/src/saltybot_rover_driver/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_rover_driver/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_rover_driver/test/test_rover_kinematics.py diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/config/rover_params.yaml b/jetson/ros2_ws/src/saltybot_rover_driver/config/rover_params.yaml new file mode 100644 index 0000000..b372c27 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_rover_driver/config/rover_params.yaml @@ -0,0 +1,26 @@ +rover_driver: + ros__parameters: + # Serial interface + serial_port: /dev/ttyUSB_rover + baud_rate: 115200 + + # Drive variant + four_wheel: true # true=4WD skid-steer, false=2WD differential + + # Physical parameters + track_width: 0.28 # left↔right wheel centre distance (m) + wheelbase: 0.30 # front↔rear axle distance (m) — used for odometry + + # Speed limits + max_speed_ms: 2.0 # m/s at full ESC deflection + max_linear_vel: 1.5 # clamp on cmd_vel linear.x (m/s) + max_angular_vel: 3.0 # clamp on cmd_vel angular.z (rad/s) + + # ESC PWM settings + pwm_neutral_us: 1500 # neutral/brake pulse width (µs) + pwm_range_us: 500 # half-range from neutral → [1000..2000] µs + + # Timing + cmd_vel_timeout_s: 0.5 # deadman: zero ESCs if /cmd_vel goes silent + heartbeat_period_s: 0.2 # H\n heartbeat interval + control_rate: 50.0 # Hz diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/launch/rover_driver.launch.py b/jetson/ros2_ws/src/saltybot_rover_driver/launch/rover_driver.launch.py new file mode 100644 index 0000000..f0c1d21 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_rover_driver/launch/rover_driver.launch.py @@ -0,0 +1,53 @@ +""" +rover_driver.launch.py — Launch the SaltyRover ESC motor driver node. + +Usage: + ros2 launch saltybot_rover_driver rover_driver.launch.py + ros2 launch saltybot_rover_driver rover_driver.launch.py serial_port:=/dev/ttyACM1 + ros2 launch saltybot_rover_driver rover_driver.launch.py four_wheel:=false +""" + +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(): + pkg_share = get_package_share_directory("saltybot_rover_driver") + params_file = os.path.join(pkg_share, "config", "rover_params.yaml") + + serial_port_arg = DeclareLaunchArgument( + "serial_port", + default_value="/dev/ttyUSB_rover", + description="Serial port for ESC controller", + ) + four_wheel_arg = DeclareLaunchArgument( + "four_wheel", + default_value="true", + description="Enable 4WD skid-steer (true) or 2WD differential (false)", + ) + + rover_driver = Node( + package="saltybot_rover_driver", + executable="rover_driver_node", + name="rover_driver", + parameters=[ + params_file, + { + "serial_port": LaunchConfiguration("serial_port"), + "four_wheel": LaunchConfiguration("four_wheel"), + }, + ], + output="screen", + emulate_tty=True, + ) + + return LaunchDescription([ + serial_port_arg, + four_wheel_arg, + rover_driver, + ]) diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/package.xml b/jetson/ros2_ws/src/saltybot_rover_driver/package.xml new file mode 100644 index 0000000..2b06582 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_rover_driver/package.xml @@ -0,0 +1,23 @@ + + + + saltybot_rover_driver + 0.1.0 + SaltyRover 4-wheel independent ESC motor driver (Issue #110) + sl-controls + MIT + + rclpy + geometry_msgs + nav_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/resource/saltybot_rover_driver b/jetson/ros2_ws/src/saltybot_rover_driver/resource/saltybot_rover_driver new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/__init__.py b/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/kinematics.py b/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/kinematics.py new file mode 100644 index 0000000..23bd5c7 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/kinematics.py @@ -0,0 +1,156 @@ +""" +kinematics.py — Differential / skid-steer kinematics for SaltyRover (Issue #110). + +Supports two drive variants selectable at runtime: + + 2-wheel (differential drive) + ──────────────────────────── + Standard unicycle model: one left motor, one right motor. + + v_left = linear_x - angular_z * track_width / 2 + v_right = linear_x + angular_z * track_width / 2 + + 4-wheel (skid steer) + ───────────────────── + Left-side motors share the same speed; right-side motors share the same speed. + The kinematic formula is identical to 2-wheel — the rover simply applies + the same left/right speeds to front and rear motors on each side. + + v_left_front = v_left + v_left_rear = v_left + v_right_front = v_right + v_right_rear = v_right + + This gives accurate straight-line and point-turn behaviour. Skid losses + during cornering are handled by the ESC/hardware; the Jetson does not + model them. + +Output +────── + Wheel speeds are returned in m/s, then clipped to ±max_wheel_speed and + converted to ESC PWM duty-cycle (µs) by the caller. + +Pure module — no ROS2 dependency; fully unit-testable. +""" + +import math + + +# ─── Wheel-speed computation ──────────────────────────────────────────────── + +def two_wheel_speeds( + linear_x: float, + angular_z: float, + track_width: float, +) -> tuple: + """ + Unicycle → differential drive. + + Parameters + ---------- + linear_x : forward velocity (m/s) + angular_z : yaw rate (rad/s, +ve = CCW / left turn) + track_width: centre-to-centre wheel separation (m) + + Returns + ------- + (v_left, v_right) in m/s + """ + half = angular_z * track_width / 2.0 + return linear_x - half, linear_x + half + + +def four_wheel_speeds( + linear_x: float, + angular_z: float, + track_width: float, +) -> tuple: + """ + Unicycle → skid-steer 4-wheel drive. + + Returns + ------- + (v_left_front, v_left_rear, v_right_front, v_right_rear) in m/s + """ + v_left, v_right = two_wheel_speeds(linear_x, angular_z, track_width) + return v_left, v_left, v_right, v_right + + +# ─── m/s → ESC PWM ───────────────────────────────────────────────────────── + +def speed_to_pwm( + speed_ms: float, + max_speed_ms: float, + pwm_neutral_us: int, + pwm_range_us: int, +) -> int: + """ + Convert wheel speed (m/s) to ESC PWM pulse width (µs). + + Linear mapping: + pwm = neutral + (speed / max_speed) * range + + Parameters + ---------- + speed_ms : desired wheel speed (m/s); negative = reverse + max_speed_ms : speed at full deflection (m/s) + pwm_neutral_us: centre / brake pulse width (µs), typically 1500 + pwm_range_us : half-range from neutral to full (µs), typically 500 + + Returns + ------- + PWM pulse width in µs, clamped to [neutral - range, neutral + range]. + """ + if max_speed_ms <= 0.0: + return pwm_neutral_us + normalised = max(-1.0, min(1.0, speed_ms / max_speed_ms)) + pwm = pwm_neutral_us + normalised * pwm_range_us + lo = pwm_neutral_us - pwm_range_us + hi = pwm_neutral_us + pwm_range_us + return int(round(max(lo, min(hi, pwm)))) + + +# ─── Variant-aware speed computation ──────────────────────────────────────── + +def compute_wheel_speeds( + linear_x: float, + angular_z: float, + track_width: float, + four_wheel: bool, + max_speed_ms: float, +) -> tuple: + """ + Compute (and clip) wheel speeds for the selected drive variant. + + Returns a tuple of clipped m/s values: + 2-wheel → (v_left, v_right) + 4-wheel → (v_left_front, v_left_rear, v_right_front, v_right_rear) + + Clipping is symmetric around zero at ±max_speed_ms. + """ + def clip(v): + return max(-max_speed_ms, min(max_speed_ms, v)) + + if four_wheel: + vfl, vlr, vrf, vrr = four_wheel_speeds(linear_x, angular_z, track_width) + return clip(vfl), clip(vlr), clip(vrf), clip(vrr) + else: + vl, vr = two_wheel_speeds(linear_x, angular_z, track_width) + return clip(vl), clip(vr) + + +# ─── Odometry helper ──────────────────────────────────────────────────────── + +def odometry_from_wheel_speeds( + v_left: float, + v_right: float, + track_width: float, +) -> tuple: + """ + Estimate linear and angular velocity from left/right wheel speeds. + + Returns (linear_x, angular_z) — the inverse of two_wheel_speeds. + """ + linear_x = (v_left + v_right) / 2.0 + angular_z = (v_right - v_left) / track_width if track_width > 0.0 else 0.0 + return linear_x, angular_z diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/rover_driver_node.py b/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/rover_driver_node.py new file mode 100644 index 0000000..ccc24b7 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/rover_driver_node.py @@ -0,0 +1,326 @@ +""" +rover_driver_node.py — SaltyRover 4-wheel independent ESC motor driver (Issue #110). + +Hardware +──────── + SaltyRover: 4-wheel ground robot with individual brushless ESCs. + ESCs controlled via PWM (servo-style 1000–2000 µs pulses). + Communication: USB CDC serial to STM32 or Raspberry Pi Pico GPIO PWM bridge. + + ESC channel assignments (configurable): + CH1 = left-front + CH2 = left-rear + CH3 = right-front + CH4 = right-rear + +Drive variants +────────────── + Selectable at runtime via `four_wheel` parameter (default True). + + 2-wheel (four_wheel=False): + Left motors share a single speed, right motors share a single speed. + Equivalent to standard differential-drive; only CH1+CH3 are used + (or all four with mirrored values, depending on hardware). + + 4-wheel skid-steer (four_wheel=True): + All four ESCs commanded independently. Front/rear same-side share speed, + making this kinematically equivalent to 2-wheel but all four motors driven. + +Serial protocol +──────────────── + Command frame (ASCII, newline-terminated): + P,,,\n + Values: 1000–2000 µs (1500 = neutral/brake) + + Heartbeat: + H\n every heartbeat_period_s (ESC controller zeros PWM after timeout) + +Subscribes +────────── + /cmd_vel (geometry_msgs/Twist) — Nav2-compatible velocity command + linear.x = forward (m/s) + angular.z = yaw rate (rad/s, +CCW) + +Publishes +───────── + /saltybot/rover_pwm (std_msgs/String JSON) — current PWM values (observability) + /saltybot/rover_odom (nav_msgs/Odometry) — wheel-speed odometry + +Parameters +────────── + serial_port /dev/ttyUSB_rover (or /dev/ttyACM1, etc.) + baud_rate 115200 + four_wheel True — runtime variant flag (toggle live via ros2 param set) + track_width 0.28 — left↔right wheel centre distance (m) + max_speed_ms 2.0 — m/s at full ESC deflection + max_linear_vel 1.5 — clamp on cmd_vel linear.x (m/s) + max_angular_vel 3.0 — clamp on cmd_vel angular.z (rad/s) + pwm_neutral_us 1500 — neutral (brake) PWM (µs) + pwm_range_us 500 — half-range from neutral (µs) → [1000..2000] + cmd_vel_timeout_s 0.5 — deadman: zero ESCs if /cmd_vel goes silent + heartbeat_period_s 0.2 + control_rate 50.0 — Hz + wheelbase 0.30 — front↔rear axle distance for odometry (m) + +Usage +───── + ros2 launch saltybot_rover_driver rover_driver.launch.py + ros2 param set /rover_driver four_wheel false # switch to 2-wheel live +""" + +import json +import math +import threading +import time + +import rclpy +from rclpy.node import Node +from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy + +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry +from std_msgs.msg import Header, String + +from saltybot_rover_driver.kinematics import compute_wheel_speeds, speed_to_pwm + +try: + import serial + _SERIAL_OK = True +except ImportError: + _SERIAL_OK = False + + +class RoverDriverNode(Node): + + def __init__(self): + super().__init__("rover_driver") + + # ── Parameters ──────────────────────────────────────────────────────── + self.declare_parameter("serial_port", "/dev/ttyUSB_rover") + self.declare_parameter("baud_rate", 115200) + self.declare_parameter("four_wheel", True) + self.declare_parameter("track_width", 0.28) + self.declare_parameter("max_speed_ms", 2.0) + self.declare_parameter("max_linear_vel", 1.5) + self.declare_parameter("max_angular_vel", 3.0) + self.declare_parameter("pwm_neutral_us", 1500) + self.declare_parameter("pwm_range_us", 500) + self.declare_parameter("cmd_vel_timeout_s", 0.5) + self.declare_parameter("heartbeat_period_s", 0.2) + self.declare_parameter("control_rate", 50.0) + self.declare_parameter("wheelbase", 0.30) + + self._p = self._load_params() + + # ── State ───────────────────────────────────────────────────────────── + self._target_linear_x: float = 0.0 + self._target_angular_z: float = 0.0 + self._last_cmd_vel_t: float = 0.0 + self._last_pwm: tuple = (1500, 1500, 1500, 1500) + self._odom_x: float = 0.0 + self._odom_y: float = 0.0 + self._odom_yaw: float = 0.0 + self._last_odom_t: float = time.monotonic() + + # ── Serial ──────────────────────────────────────────────────────────── + self._ser = None + self._ser_lock = threading.Lock() + if _SERIAL_OK: + self._open_serial() + else: + self.get_logger().warn( + "pyserial not installed — running in simulation mode (no serial I/O)") + + # ── QoS ─────────────────────────────────────────────────────────────── + reliable = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + ) + + # ── Subscriptions ───────────────────────────────────────────────────── + self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, reliable) + + # ── Publishers ──────────────────────────────────────────────────────── + self._pwm_pub = self.create_publisher(String, "/saltybot/rover_pwm", reliable) + self._odom_pub = self.create_publisher(Odometry, "/saltybot/rover_odom", reliable) + + # ── Timers ──────────────────────────────────────────────────────────── + rate = self._p["control_rate"] + self._ctrl_timer = self.create_timer(1.0 / rate, self._control_cb) + self._hb_timer = self.create_timer(self._p["hb_period"], self._heartbeat_cb) + + self.get_logger().info( + f"RoverDriverNode ready " + f"variant={'4WD' if self._p['four_wheel'] else '2WD'} " + f"track={self._p['track_width']}m " + f"max_speed={self._p['max_speed_ms']}m/s " + f"port={self._p['serial_port']}" + ) + + # ── Parameter load ──────────────────────────────────────────────────────── + + def _load_params(self) -> dict: + return { + "serial_port": self.get_parameter("serial_port").value, + "baud_rate": self.get_parameter("baud_rate").value, + "four_wheel": self.get_parameter("four_wheel").value, + "track_width": self.get_parameter("track_width").value, + "max_speed_ms": self.get_parameter("max_speed_ms").value, + "max_linear_vel": self.get_parameter("max_linear_vel").value, + "max_angular_vel":self.get_parameter("max_angular_vel").value, + "pwm_neutral": int(self.get_parameter("pwm_neutral_us").value), + "pwm_range": int(self.get_parameter("pwm_range_us").value), + "cmd_timeout": self.get_parameter("cmd_vel_timeout_s").value, + "hb_period": self.get_parameter("heartbeat_period_s").value, + "control_rate": self.get_parameter("control_rate").value, + "wheelbase": self.get_parameter("wheelbase").value, + } + + # ── Serial helpers ──────────────────────────────────────────────────────── + + def _open_serial(self) -> None: + with self._ser_lock: + try: + self._ser = serial.Serial( + self._p["serial_port"], self._p["baud_rate"], timeout=0.05) + self.get_logger().info(f"Serial open: {self._p['serial_port']}") + except Exception as exc: + self.get_logger().error(f"Cannot open serial: {exc}") + self._ser = None + + def _write(self, data: bytes) -> bool: + with self._ser_lock: + if self._ser is None or not self._ser.is_open: + return False + try: + self._ser.write(data) + return True + except Exception as exc: + self.get_logger().error(f"Serial write: {exc}", throttle_duration_sec=2.0) + self._ser = None + return False + + # ── cmd_vel callback ────────────────────────────────────────────────────── + + def _cmd_vel_cb(self, msg: Twist) -> None: + p = self._p + self._target_linear_x = max(-p["max_linear_vel"], + min(p["max_linear_vel"], msg.linear.x)) + self._target_angular_z = max(-p["max_angular_vel"], + min(p["max_angular_vel"], msg.angular.z)) + self._last_cmd_vel_t = time.monotonic() + + # ── 50 Hz control loop ──────────────────────────────────────────────────── + + def _control_cb(self) -> None: + self._p = self._load_params() + p = self._p + now = time.monotonic() + + # Deadman + cmd_age = (now - self._last_cmd_vel_t) if self._last_cmd_vel_t > 0.0 else 1e9 + if cmd_age > p["cmd_timeout"]: + lin_x, ang_z = 0.0, 0.0 + else: + lin_x, ang_z = self._target_linear_x, self._target_angular_z + + # Kinematics → wheel speeds + speeds = compute_wheel_speeds( + lin_x, ang_z, + track_width=p["track_width"], + four_wheel=p["four_wheel"], + max_speed_ms=p["max_speed_ms"], + ) + + # Pad to 4 values for protocol; 2-wheel mirrors front to rear + if len(speeds) == 2: + vl, vr = speeds + speeds4 = (vl, vl, vr, vr) + else: + speeds4 = speeds # (vfl, vlr, vrf, vrr) + + # Convert to PWM + def to_pwm(v): + return speed_to_pwm(v, p["max_speed_ms"], p["pwm_neutral"], p["pwm_range"]) + + pwm = tuple(to_pwm(v) for v in speeds4) + self._last_pwm = pwm + + # Send P command + frame = f"P{pwm[0]},{pwm[1]},{pwm[2]},{pwm[3]}\n".encode("ascii") + self._write(frame) + + # Publish observability + pwm_msg = String() + pwm_msg.data = json.dumps({ + "ch1_us": pwm[0], "ch2_us": pwm[1], + "ch3_us": pwm[2], "ch4_us": pwm[3], + "variant": "4WD" if p["four_wheel"] else "2WD", + }) + self._pwm_pub.publish(pwm_msg) + + # Odometry integration (dead reckoning) + dt = now - self._last_odom_t + self._last_odom_t = now + self._integrate_odometry(speeds4[0], speeds4[2], p["track_width"], dt) + self._publish_odom(now) + + # ── Heartbeat ───────────────────────────────────────────────────────────── + + def _heartbeat_cb(self) -> None: + self._write(b"H\n") + + # ── Odometry ───────────────────────────────────────────────────────────── + + def _integrate_odometry( + self, v_left: float, v_right: float, track_width: float, dt: float + ) -> None: + """Simple 2D dead-reckoning from left/right motor speeds.""" + lin = (v_left + v_right) / 2.0 + ang = (v_right - v_left) / track_width if track_width > 0.0 else 0.0 + self._odom_yaw += ang * dt + self._odom_x += lin * math.cos(self._odom_yaw) * dt + self._odom_y += lin * math.sin(self._odom_yaw) * dt + + def _publish_odom(self, now: float) -> None: + yaw = self._odom_yaw + msg = Odometry() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "odom" + msg.child_frame_id = "base_link" + + msg.pose.pose.position.x = self._odom_x + msg.pose.pose.position.y = self._odom_y + msg.pose.pose.orientation.z = math.sin(yaw / 2.0) + msg.pose.pose.orientation.w = math.cos(yaw / 2.0) + + lin_x = (self._last_pwm[0] + self._last_pwm[2]) / 2.0 # rough proxy + msg.twist.twist.linear.x = self._target_linear_x + msg.twist.twist.angular.z = self._target_angular_z + + self._odom_pub.publish(msg) + + # ── Lifecycle ───────────────────────────────────────────────────────────── + + def destroy_node(self) -> None: + self._write(b"P1500,1500,1500,1500\n") # neutral on shutdown + super().destroy_node() + + +# ── Entry point ─────────────────────────────────────────────────────────────── + +def main(args=None): + rclpy.init(args=args) + node = RoverDriverNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/setup.cfg b/jetson/ros2_ws/src/saltybot_rover_driver/setup.cfg new file mode 100644 index 0000000..ac99598 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_rover_driver/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/saltybot_rover_driver + +[install] +install_scripts=$base/lib/saltybot_rover_driver diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/setup.py b/jetson/ros2_ws/src/saltybot_rover_driver/setup.py new file mode 100644 index 0000000..cc1a459 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_rover_driver/setup.py @@ -0,0 +1,32 @@ +from setuptools import setup, find_packages +import os +from glob import glob + +package_name = "saltybot_rover_driver" + +setup( + name=package_name, + version="0.1.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", + [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + (os.path.join("share", package_name, "config"), + glob("config/*.yaml")), + (os.path.join("share", package_name, "launch"), + glob("launch/*.py")), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="sl-controls", + maintainer_email="sl-controls@saltylab.local", + description="SaltyRover 4-wheel independent ESC motor driver", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + f"rover_driver_node = {package_name}.rover_driver_node:main", + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_rover_driver/test/test_rover_kinematics.py b/jetson/ros2_ws/src/saltybot_rover_driver/test/test_rover_kinematics.py new file mode 100644 index 0000000..ac54f48 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_rover_driver/test/test_rover_kinematics.py @@ -0,0 +1,295 @@ +""" +test_rover_kinematics.py — Unit tests for kinematics.py (Issue #110). + +No ROS2 runtime required — pure Python. +""" + +import math +import pytest + +from saltybot_rover_driver.kinematics import ( + two_wheel_speeds, + four_wheel_speeds, + speed_to_pwm, + compute_wheel_speeds, + odometry_from_wheel_speeds, +) + + +# ─── two_wheel_speeds ──────────────────────────────────────────────────────── + +class TestTwoWheelSpeeds: + + def test_straight_ahead(self): + vl, vr = two_wheel_speeds(1.0, 0.0, 0.28) + assert vl == pytest.approx(1.0) + assert vr == pytest.approx(1.0) + + def test_straight_reverse(self): + vl, vr = two_wheel_speeds(-1.0, 0.0, 0.28) + assert vl == pytest.approx(-1.0) + assert vr == pytest.approx(-1.0) + + def test_point_turn_ccw(self): + """Pure CCW rotation: linear_x=0, angular_z>0 → left slower, right faster.""" + vl, vr = two_wheel_speeds(0.0, 1.0, 0.28) + assert vl == pytest.approx(-0.14) + assert vr == pytest.approx(0.14) + + def test_point_turn_cw(self): + """Pure CW rotation: linear_x=0, angular_z<0 → left faster, right slower.""" + vl, vr = two_wheel_speeds(0.0, -1.0, 0.28) + assert vl == pytest.approx(0.14) + assert vr == pytest.approx(-0.14) + + def test_forward_left_curve(self): + vl, vr = two_wheel_speeds(1.0, 0.5, 0.28) + assert vl == pytest.approx(1.0 - 0.5 * 0.28 / 2.0) + assert vr == pytest.approx(1.0 + 0.5 * 0.28 / 2.0) + + def test_zero_inputs(self): + vl, vr = two_wheel_speeds(0.0, 0.0, 0.28) + assert vl == pytest.approx(0.0) + assert vr == pytest.approx(0.0) + + def test_zero_track_width(self): + """track_width=0 should not crash; both wheels get linear_x.""" + vl, vr = two_wheel_speeds(1.0, 1.0, 0.0) + assert vl == pytest.approx(1.0) + assert vr == pytest.approx(1.0) + + def test_symmetry(self): + """Reversing angular_z mirrors left/right.""" + vl1, vr1 = two_wheel_speeds(0.5, 0.3, 0.28) + vl2, vr2 = two_wheel_speeds(0.5, -0.3, 0.28) + assert vl1 == pytest.approx(vr2) + assert vr1 == pytest.approx(vl2) + + +# ─── four_wheel_speeds ─────────────────────────────────────────────────────── + +class TestFourWheelSpeeds: + + def test_straight(self): + vfl, vlr, vrf, vrr = four_wheel_speeds(1.0, 0.0, 0.28) + assert vfl == pytest.approx(1.0) + assert vlr == pytest.approx(1.0) + assert vrf == pytest.approx(1.0) + assert vrr == pytest.approx(1.0) + + def test_front_rear_share_speed(self): + """Front and rear on each side must have identical speeds.""" + vfl, vlr, vrf, vrr = four_wheel_speeds(1.0, 0.5, 0.28) + assert vfl == pytest.approx(vlr) + assert vrf == pytest.approx(vrr) + + def test_point_turn(self): + vfl, vlr, vrf, vrr = four_wheel_speeds(0.0, 1.0, 0.28) + expected_l = -0.14 + expected_r = 0.14 + assert vfl == pytest.approx(expected_l) + assert vlr == pytest.approx(expected_l) + assert vrf == pytest.approx(expected_r) + assert vrr == pytest.approx(expected_r) + + def test_returns_four_values(self): + result = four_wheel_speeds(1.0, 0.3, 0.28) + assert len(result) == 4 + + def test_consistent_with_two_wheel(self): + """4-wheel is just 2-wheel spread to four channels.""" + vl, vr = two_wheel_speeds(0.8, 0.4, 0.30) + vfl, vlr, vrf, vrr = four_wheel_speeds(0.8, 0.4, 0.30) + assert vfl == pytest.approx(vl) + assert vlr == pytest.approx(vl) + assert vrf == pytest.approx(vr) + assert vrr == pytest.approx(vr) + + +# ─── speed_to_pwm ──────────────────────────────────────────────────────────── + +class TestSpeedToPwm: + + def test_zero_speed_is_neutral(self): + assert speed_to_pwm(0.0, 2.0, 1500, 500) == 1500 + + def test_full_forward(self): + assert speed_to_pwm(2.0, 2.0, 1500, 500) == 2000 + + def test_full_reverse(self): + assert speed_to_pwm(-2.0, 2.0, 1500, 500) == 1000 + + def test_half_forward(self): + assert speed_to_pwm(1.0, 2.0, 1500, 500) == 1750 + + def test_half_reverse(self): + assert speed_to_pwm(-1.0, 2.0, 1500, 500) == 1250 + + def test_over_max_clamps_to_2000(self): + assert speed_to_pwm(5.0, 2.0, 1500, 500) == 2000 + + def test_under_min_clamps_to_1000(self): + assert speed_to_pwm(-5.0, 2.0, 1500, 500) == 1000 + + def test_zero_max_speed_returns_neutral(self): + assert speed_to_pwm(1.0, 0.0, 1500, 500) == 1500 + + def test_negative_max_speed_returns_neutral(self): + assert speed_to_pwm(1.0, -1.0, 1500, 500) == 1500 + + def test_returns_int(self): + result = speed_to_pwm(0.5, 2.0, 1500, 500) + assert isinstance(result, int) + + def test_custom_neutral_and_range(self): + """Supports non-standard neutral/range values.""" + pwm = speed_to_pwm(1.0, 2.0, 1400, 400) + assert pwm == 1600 # 1400 + 0.5*400 + + def test_quarter_speed(self): + pwm = speed_to_pwm(0.5, 2.0, 1500, 500) + assert pwm == 1625 # 1500 + 0.25*500 + + def test_rounding(self): + """Result should be integer-rounded, not truncated.""" + # speed=1/3 of max → 1500 + (1/3)*500 = 1666.67 → rounds to 1667 + pwm = speed_to_pwm(2.0 / 3.0, 2.0, 1500, 500) + assert pwm == 1667 + + +# ─── compute_wheel_speeds ──────────────────────────────────────────────────── + +class TestComputeWheelSpeeds: + + def test_two_wheel_mode_returns_two_values(self): + result = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0) + assert len(result) == 2 + + def test_four_wheel_mode_returns_four_values(self): + result = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=True, max_speed_ms=2.0) + assert len(result) == 4 + + def test_clipping_two_wheel(self): + """Sharp turn at full speed: outer wheel must be clipped to max.""" + result = compute_wheel_speeds(2.0, 5.0, 0.28, four_wheel=False, max_speed_ms=2.0) + vl, vr = result + assert vl >= -2.0 + assert vr <= 2.0 + + def test_clipping_four_wheel(self): + result = compute_wheel_speeds(2.0, 5.0, 0.28, four_wheel=True, max_speed_ms=2.0) + for v in result: + assert -2.0 <= v <= 2.0 + + def test_straight_no_clip(self): + result = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0) + assert result == pytest.approx((1.0, 1.0)) + + def test_four_wheel_symmetric(self): + vfl, vlr, vrf, vrr = compute_wheel_speeds(0.5, 0.2, 0.28, four_wheel=True, max_speed_ms=2.0) + assert vfl == pytest.approx(vlr) + assert vrf == pytest.approx(vrr) + + def test_zero_inputs_two_wheel(self): + result = compute_wheel_speeds(0.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0) + assert result == pytest.approx((0.0, 0.0)) + + def test_zero_inputs_four_wheel(self): + result = compute_wheel_speeds(0.0, 0.0, 0.28, four_wheel=True, max_speed_ms=2.0) + assert result == pytest.approx((0.0, 0.0, 0.0, 0.0)) + + def test_negative_speed_two_wheel(self): + vl, vr = compute_wheel_speeds(-1.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0) + assert vl == pytest.approx(-1.0) + assert vr == pytest.approx(-1.0) + + +# ─── odometry_from_wheel_speeds ────────────────────────────────────────────── + +class TestOdometryFromWheelSpeeds: + + def test_straight(self): + lin, ang = odometry_from_wheel_speeds(1.0, 1.0, 0.28) + assert lin == pytest.approx(1.0) + assert ang == pytest.approx(0.0) + + def test_point_turn_ccw(self): + """Equal and opposite: left=-v, right=+v → angular = 2v/track.""" + v = 0.14 + lin, ang = odometry_from_wheel_speeds(-v, v, 0.28) + assert lin == pytest.approx(0.0) + assert ang == pytest.approx(1.0) + + def test_point_turn_cw(self): + v = 0.14 + lin, ang = odometry_from_wheel_speeds(v, -v, 0.28) + assert lin == pytest.approx(0.0) + assert ang == pytest.approx(-1.0) + + def test_zero_inputs(self): + lin, ang = odometry_from_wheel_speeds(0.0, 0.0, 0.28) + assert lin == pytest.approx(0.0) + assert ang == pytest.approx(0.0) + + def test_zero_track_width_no_crash(self): + lin, ang = odometry_from_wheel_speeds(1.0, 1.0, 0.0) + assert lin == pytest.approx(1.0) + assert ang == pytest.approx(0.0) + + def test_inverse_of_two_wheel_speeds(self): + """odometry_from_wheel_speeds should be the inverse of two_wheel_speeds.""" + lin_in, ang_in = 0.7, 0.5 + track = 0.28 + vl, vr = two_wheel_speeds(lin_in, ang_in, track) + lin_out, ang_out = odometry_from_wheel_speeds(vl, vr, track) + assert lin_out == pytest.approx(lin_in) + assert ang_out == pytest.approx(ang_in) + + def test_reverse(self): + lin, ang = odometry_from_wheel_speeds(-1.0, -1.0, 0.28) + assert lin == pytest.approx(-1.0) + assert ang == pytest.approx(0.0) + + +# ─── Integration: kinematics round-trip ───────────────────────────────────── + +class TestRoundTrip: + + @pytest.mark.parametrize("lin,ang", [ + (1.0, 0.0), + (0.0, 1.0), + (0.5, 0.5), + (-0.5, 0.3), + (1.0, -0.8), + ]) + def test_roundtrip_two_wheel(self, lin, ang): + """compute → odometry_from → same (lin, ang) for unclipped inputs.""" + track = 0.28 + vl, vr = compute_wheel_speeds(lin, ang, track, four_wheel=False, max_speed_ms=2.0) + lin_out, ang_out = odometry_from_wheel_speeds(vl, vr, track) + assert lin_out == pytest.approx(lin, abs=1e-9) + assert ang_out == pytest.approx(ang, abs=1e-9) + + def test_pwm_neutral_at_zero_speed(self): + pwm = speed_to_pwm(0.0, 2.0, 1500, 500) + assert pwm == 1500 + + def test_pwm_full_deflection_forward(self): + vl, vr = compute_wheel_speeds(2.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0) + pwm_l = speed_to_pwm(vl, 2.0, 1500, 500) + pwm_r = speed_to_pwm(vr, 2.0, 1500, 500) + assert pwm_l == 2000 + assert pwm_r == 2000 + + def test_pwm_full_deflection_reverse(self): + vl, vr = compute_wheel_speeds(-2.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0) + pwm_l = speed_to_pwm(vl, 2.0, 1500, 500) + pwm_r = speed_to_pwm(vr, 2.0, 1500, 500) + assert pwm_l == 1000 + assert pwm_r == 1000 + + def test_four_wheel_all_channels_driven(self): + """All four PWMs should be non-neutral during forward motion.""" + vfl, vlr, vrf, vrr = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=True, max_speed_ms=2.0) + pwms = [speed_to_pwm(v, 2.0, 1500, 500) for v in (vfl, vlr, vrf, vrr)] + assert all(p > 1500 for p in pwms) -- 2.47.2