feat(rover): SaltyRover 4-wheel ESC motor driver (Issue #110)
- 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<ch1>,<ch2>,<ch3>,<ch4>\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 <noreply@anthropic.com>
This commit is contained in:
parent
6a30b20aaa
commit
e313d92012
@ -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
|
||||
@ -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,
|
||||
])
|
||||
23
jetson/ros2_ws/src/saltybot_rover_driver/package.xml
Normal file
23
jetson/ros2_ws/src/saltybot_rover_driver/package.xml
Normal file
@ -0,0 +1,23 @@
|
||||
<?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_rover_driver</name>
|
||||
<version>0.1.0</version>
|
||||
<description>SaltyRover 4-wheel independent ESC motor driver (Issue #110)</description>
|
||||
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -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
|
||||
@ -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<ch1_us>,<ch2_us>,<ch3_us>,<ch4_us>\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()
|
||||
5
jetson/ros2_ws/src/saltybot_rover_driver/setup.cfg
Normal file
5
jetson/ros2_ws/src/saltybot_rover_driver/setup.cfg
Normal file
@ -0,0 +1,5 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_rover_driver
|
||||
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_rover_driver
|
||||
32
jetson/ros2_ws/src/saltybot_rover_driver/setup.py
Normal file
32
jetson/ros2_ws/src/saltybot_rover_driver/setup.py
Normal file
@ -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",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -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)
|
||||
Loading…
x
Reference in New Issue
Block a user