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)