Merge pull request 'feat: VESC MQTT telemetry relay (Issue #656)' (#660) from sl-android/issue-656-vesc-mqtt-relay into main
This commit is contained in:
commit
6561e35fd6
@ -0,0 +1,305 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
vesc_mqtt_relay_node.py — ROS2 → MQTT relay for VESC CAN telemetry (Issue #656)
|
||||
|
||||
Subscribes to VESC telemetry ROS2 topics and republishes as MQTT JSON payloads
|
||||
for the sensor dashboard. Each per-motor topic is rate-limited to 5 Hz.
|
||||
|
||||
ROS2 topics consumed
|
||||
─────────────────────
|
||||
/vesc/left/state std_msgs/String JSON from vesc_telemetry_node
|
||||
/vesc/right/state std_msgs/String JSON from vesc_telemetry_node
|
||||
/vesc/combined std_msgs/String JSON from vesc_telemetry_node
|
||||
|
||||
MQTT topics published
|
||||
──────────────────────
|
||||
saltybot/phone/vesc_left — per-motor telemetry (left)
|
||||
saltybot/phone/vesc_right — per-motor telemetry (right)
|
||||
saltybot/phone/vesc_combined — combined voltage + total current + RPMs
|
||||
|
||||
MQTT payload (per-motor)
|
||||
────────────────────────
|
||||
{
|
||||
"rpm": int,
|
||||
"current_a": float, # phase current
|
||||
"voltage_v": float, # bus voltage
|
||||
"temperature_c": float, # FET temperature
|
||||
"duty_cycle": float, # -1.0 … 1.0
|
||||
"fault_code": int,
|
||||
"ts": float # epoch seconds
|
||||
}
|
||||
|
||||
Parameters
|
||||
──────────
|
||||
mqtt_host str Broker IP/hostname (default: localhost)
|
||||
mqtt_port int Broker port (default: 1883)
|
||||
mqtt_keepalive int Keepalive seconds (default: 60)
|
||||
reconnect_delay_s float Delay between reconnects (default: 5.0)
|
||||
motor_rate_hz float Max publish rate per motor (default: 5.0)
|
||||
|
||||
Dependencies
|
||||
────────────
|
||||
pip install paho-mqtt
|
||||
"""
|
||||
|
||||
import json
|
||||
import time
|
||||
from typing import Any
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import String
|
||||
|
||||
try:
|
||||
import paho.mqtt.client as mqtt
|
||||
_MQTT_OK = True
|
||||
except ImportError:
|
||||
_MQTT_OK = False
|
||||
|
||||
# ── MQTT topic constants ──────────────────────────────────────────────────────
|
||||
|
||||
_MQTT_VESC_LEFT = "saltybot/phone/vesc_left"
|
||||
_MQTT_VESC_RIGHT = "saltybot/phone/vesc_right"
|
||||
_MQTT_VESC_COMBINED = "saltybot/phone/vesc_combined"
|
||||
|
||||
# ── ROS2 topic constants ──────────────────────────────────────────────────────
|
||||
|
||||
_ROS_VESC_LEFT = "/vesc/left/state"
|
||||
_ROS_VESC_RIGHT = "/vesc/right/state"
|
||||
_ROS_VESC_COMBINED = "/vesc/combined"
|
||||
|
||||
|
||||
def _extract_motor_payload(data: dict) -> dict:
|
||||
"""
|
||||
Extract the required fields from a vesc_telemetry_node per-motor JSON dict.
|
||||
|
||||
Upstream JSON keys (from vesc_telemetry_node._state_to_dict):
|
||||
rpm, current_a, voltage_v, temp_fet_c, duty_cycle, fault_code, ...
|
||||
|
||||
Returns a dashboard-friendly payload with a stable key set.
|
||||
"""
|
||||
return {
|
||||
"rpm": int(data["rpm"]),
|
||||
"current_a": float(data["current_a"]),
|
||||
"voltage_v": float(data["voltage_v"]),
|
||||
"temperature_c": float(data["temp_fet_c"]),
|
||||
"duty_cycle": float(data["duty_cycle"]),
|
||||
"fault_code": int(data["fault_code"]),
|
||||
"ts": float(data.get("stamp", time.time())),
|
||||
}
|
||||
|
||||
|
||||
def _extract_combined_payload(data: dict) -> dict:
|
||||
"""
|
||||
Extract fields from the /vesc/combined JSON dict.
|
||||
|
||||
Upstream keys: voltage_v, total_current_a, left_rpm, right_rpm,
|
||||
left_alive, right_alive, stamp
|
||||
"""
|
||||
return {
|
||||
"voltage_v": float(data["voltage_v"]),
|
||||
"total_current_a": float(data["total_current_a"]),
|
||||
"left_rpm": int(data["left_rpm"]),
|
||||
"right_rpm": int(data["right_rpm"]),
|
||||
"left_alive": bool(data["left_alive"]),
|
||||
"right_alive": bool(data["right_alive"]),
|
||||
"ts": float(data.get("stamp", time.time())),
|
||||
}
|
||||
|
||||
|
||||
class VescMqttRelayNode(Node):
|
||||
"""
|
||||
Subscribes to VESC ROS2 topics and relays telemetry to MQTT.
|
||||
|
||||
Rate limiting: each per-motor topic maintains a last-publish timestamp;
|
||||
messages arriving faster than motor_rate_hz are silently dropped.
|
||||
The /vesc/combined topic is also rate-limited at the same rate.
|
||||
"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("vesc_mqtt_relay")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────────
|
||||
self.declare_parameter("mqtt_host", "localhost")
|
||||
self.declare_parameter("mqtt_port", 1883)
|
||||
self.declare_parameter("mqtt_keepalive", 60)
|
||||
self.declare_parameter("reconnect_delay_s", 5.0)
|
||||
self.declare_parameter("motor_rate_hz", 5.0)
|
||||
|
||||
self._mqtt_host = self.get_parameter("mqtt_host").value
|
||||
self._mqtt_port = self.get_parameter("mqtt_port").value
|
||||
self._mqtt_keepalive = self.get_parameter("mqtt_keepalive").value
|
||||
reconnect_delay = self.get_parameter("reconnect_delay_s").value
|
||||
rate_hz = max(0.1, float(self.get_parameter("motor_rate_hz").value))
|
||||
self._min_interval = 1.0 / rate_hz
|
||||
|
||||
if not _MQTT_OK:
|
||||
self.get_logger().error(
|
||||
"paho-mqtt not installed — run: pip install paho-mqtt"
|
||||
)
|
||||
|
||||
# ── Rate-limit state (last publish epoch per MQTT topic) ──────────────
|
||||
self._last_pub: dict[str, float] = {
|
||||
_MQTT_VESC_LEFT: 0.0,
|
||||
_MQTT_VESC_RIGHT: 0.0,
|
||||
_MQTT_VESC_COMBINED: 0.0,
|
||||
}
|
||||
|
||||
# ── Stats ─────────────────────────────────────────────────────────────
|
||||
self._rx_count: dict[str, int] = {
|
||||
_MQTT_VESC_LEFT: 0, _MQTT_VESC_RIGHT: 0, _MQTT_VESC_COMBINED: 0
|
||||
}
|
||||
self._pub_count: dict[str, int] = {
|
||||
_MQTT_VESC_LEFT: 0, _MQTT_VESC_RIGHT: 0, _MQTT_VESC_COMBINED: 0
|
||||
}
|
||||
self._drop_count: dict[str, int] = {
|
||||
_MQTT_VESC_LEFT: 0, _MQTT_VESC_RIGHT: 0, _MQTT_VESC_COMBINED: 0
|
||||
}
|
||||
self._err_count: dict[str, int] = {
|
||||
_MQTT_VESC_LEFT: 0, _MQTT_VESC_RIGHT: 0, _MQTT_VESC_COMBINED: 0
|
||||
}
|
||||
self._mqtt_connected = False
|
||||
|
||||
# ── ROS2 subscriptions ────────────────────────────────────────────────
|
||||
self.create_subscription(
|
||||
String, _ROS_VESC_LEFT,
|
||||
lambda msg: self._on_vesc(msg, _MQTT_VESC_LEFT, _extract_motor_payload),
|
||||
10,
|
||||
)
|
||||
self.create_subscription(
|
||||
String, _ROS_VESC_RIGHT,
|
||||
lambda msg: self._on_vesc(msg, _MQTT_VESC_RIGHT, _extract_motor_payload),
|
||||
10,
|
||||
)
|
||||
self.create_subscription(
|
||||
String, _ROS_VESC_COMBINED,
|
||||
lambda msg: self._on_vesc(msg, _MQTT_VESC_COMBINED, _extract_combined_payload),
|
||||
10,
|
||||
)
|
||||
|
||||
# ── MQTT client ───────────────────────────────────────────────────────
|
||||
self._mqtt_client: "mqtt.Client | None" = None
|
||||
if _MQTT_OK:
|
||||
self._mqtt_client = mqtt.Client(
|
||||
client_id="saltybot-vesc-mqtt-relay", clean_session=True
|
||||
)
|
||||
self._mqtt_client.on_connect = self._on_mqtt_connect
|
||||
self._mqtt_client.on_disconnect = self._on_mqtt_disconnect
|
||||
self._mqtt_client.reconnect_delay_set(
|
||||
min_delay=int(reconnect_delay),
|
||||
max_delay=int(reconnect_delay) * 4,
|
||||
)
|
||||
self._mqtt_connect()
|
||||
|
||||
self.get_logger().info(
|
||||
"VESC→MQTT relay started — broker=%s:%d rate=%.1f Hz",
|
||||
self._mqtt_host, self._mqtt_port, rate_hz,
|
||||
)
|
||||
|
||||
# ── MQTT connection ───────────────────────────────────────────────────────
|
||||
|
||||
def _mqtt_connect(self) -> None:
|
||||
try:
|
||||
self._mqtt_client.connect_async(
|
||||
self._mqtt_host, self._mqtt_port,
|
||||
keepalive=self._mqtt_keepalive,
|
||||
)
|
||||
self._mqtt_client.loop_start()
|
||||
except Exception as exc:
|
||||
self.get_logger().warning("MQTT initial connect error: %s", str(exc))
|
||||
|
||||
def _on_mqtt_connect(self, client, userdata, flags, rc) -> None:
|
||||
if rc == 0:
|
||||
self._mqtt_connected = True
|
||||
self.get_logger().info(
|
||||
"MQTT connected to %s:%d", self._mqtt_host, self._mqtt_port
|
||||
)
|
||||
else:
|
||||
self.get_logger().warning("MQTT connect failed rc=%d", rc)
|
||||
|
||||
def _on_mqtt_disconnect(self, client, userdata, rc) -> None:
|
||||
self._mqtt_connected = False
|
||||
if rc != 0:
|
||||
self.get_logger().warning(
|
||||
"MQTT disconnected (rc=%d) — paho will reconnect", rc
|
||||
)
|
||||
|
||||
# ── ROS2 subscriber callback ──────────────────────────────────────────────
|
||||
|
||||
def _on_vesc(self, ros_msg: String, mqtt_topic: str, extractor) -> None:
|
||||
"""
|
||||
Handle an incoming VESC ROS2 message.
|
||||
|
||||
1. Parse JSON from the String payload.
|
||||
2. Check rate limit — drop if too soon.
|
||||
3. Extract dashboard fields.
|
||||
4. Publish to MQTT.
|
||||
"""
|
||||
self._rx_count[mqtt_topic] += 1
|
||||
|
||||
# Rate limit
|
||||
now = time.monotonic()
|
||||
if now - self._last_pub[mqtt_topic] < self._min_interval:
|
||||
self._drop_count[mqtt_topic] += 1
|
||||
return
|
||||
|
||||
# Parse
|
||||
try:
|
||||
data = json.loads(ros_msg.data)
|
||||
except (json.JSONDecodeError, UnicodeDecodeError) as exc:
|
||||
self._err_count[mqtt_topic] += 1
|
||||
self.get_logger().debug("JSON error on %s: %s", mqtt_topic, exc)
|
||||
return
|
||||
|
||||
# Extract
|
||||
try:
|
||||
payload = extractor(data)
|
||||
except (KeyError, TypeError, ValueError) as exc:
|
||||
self._err_count[mqtt_topic] += 1
|
||||
self.get_logger().debug("Payload error on %s: %s — %s", mqtt_topic, exc, data)
|
||||
return
|
||||
|
||||
# Publish
|
||||
if self._mqtt_client is not None and self._mqtt_connected:
|
||||
try:
|
||||
self._mqtt_client.publish(
|
||||
mqtt_topic,
|
||||
json.dumps(payload, separators=(",", ":")),
|
||||
qos=0,
|
||||
retain=False,
|
||||
)
|
||||
self._last_pub[mqtt_topic] = now
|
||||
self._pub_count[mqtt_topic] += 1
|
||||
except Exception as exc:
|
||||
self._err_count[mqtt_topic] += 1
|
||||
self.get_logger().debug("MQTT publish error on %s: %s", mqtt_topic, exc)
|
||||
else:
|
||||
# Still update last_pub to avoid log spam when broker is down
|
||||
self._last_pub[mqtt_topic] = now
|
||||
self.get_logger().debug("MQTT not connected — dropped %s", mqtt_topic)
|
||||
|
||||
# ── Cleanup ───────────────────────────────────────────────────────────────
|
||||
|
||||
def destroy_node(self) -> None:
|
||||
if self._mqtt_client is not None:
|
||||
self._mqtt_client.loop_stop()
|
||||
self._mqtt_client.disconnect()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
# ── Entry point ───────────────────────────────────────────────────────────────
|
||||
|
||||
def main(args: Any = None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = VescMqttRelayNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -30,6 +30,7 @@ setup(
|
||||
'phone_bridge = saltybot_phone.ws_bridge:main',
|
||||
'phone_camera_node = saltybot_phone.phone_camera_node:main',
|
||||
'mqtt_ros2_bridge = saltybot_phone.mqtt_ros2_bridge_node:main',
|
||||
'vesc_mqtt_relay = saltybot_phone.vesc_mqtt_relay_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
0
jetson/ros2_ws/src/saltybot_phone/test/__init__.py
Normal file
0
jetson/ros2_ws/src/saltybot_phone/test/__init__.py
Normal file
343
jetson/ros2_ws/src/saltybot_phone/test/test_vesc_mqtt_relay.py
Normal file
343
jetson/ros2_ws/src/saltybot_phone/test/test_vesc_mqtt_relay.py
Normal file
@ -0,0 +1,343 @@
|
||||
"""Unit tests for vesc_mqtt_relay_node — pure-logic helpers.
|
||||
|
||||
Does not require ROS2, paho-mqtt, or a live VESC/broker.
|
||||
Tests cover the two payload extractors and the rate-limiting logic.
|
||||
"""
|
||||
|
||||
import json
|
||||
import sys
|
||||
import time
|
||||
import types
|
||||
|
||||
# ── Stub out ROS2 / paho so the module can be imported without them ───────────
|
||||
|
||||
def _make_rclpy_stub():
|
||||
rclpy = types.ModuleType("rclpy")
|
||||
node_mod = types.ModuleType("rclpy.node")
|
||||
|
||||
class _Node:
|
||||
def __init__(self, *a, **kw): pass
|
||||
def declare_parameter(self, *a, **kw): pass
|
||||
def get_parameter(self, name):
|
||||
class _P:
|
||||
value = None
|
||||
return _P()
|
||||
def create_subscription(self, *a, **kw): pass
|
||||
def create_publisher(self, *a, **kw): pass
|
||||
def create_timer(self, *a, **kw): pass
|
||||
def get_clock(self): return None
|
||||
def get_logger(self):
|
||||
class _L:
|
||||
def info(self, *a, **kw): pass
|
||||
def warning(self, *a, **kw): pass
|
||||
def error(self, *a, **kw): pass
|
||||
def debug(self, *a, **kw): pass
|
||||
return _L()
|
||||
def destroy_node(self): pass
|
||||
|
||||
node_mod.Node = _Node
|
||||
rclpy.node = node_mod
|
||||
rclpy.init = lambda *a, **kw: None
|
||||
rclpy.spin = lambda *a, **kw: None
|
||||
rclpy.try_shutdown = lambda *a, **kw: None
|
||||
|
||||
std_msgs_mod = types.ModuleType("std_msgs")
|
||||
std_msgs_msg = types.ModuleType("std_msgs.msg")
|
||||
class _String:
|
||||
data: str = ""
|
||||
std_msgs_msg.String = _String
|
||||
std_msgs_mod.msg = std_msgs_msg
|
||||
|
||||
paho_mod = types.ModuleType("paho")
|
||||
paho_mqtt = types.ModuleType("paho.mqtt")
|
||||
paho_client = types.ModuleType("paho.mqtt.client")
|
||||
class _Client:
|
||||
def __init__(self, *a, **kw): pass
|
||||
def connect_async(self, *a, **kw): pass
|
||||
def loop_start(self): pass
|
||||
def loop_stop(self): pass
|
||||
def disconnect(self): pass
|
||||
def publish(self, *a, **kw): pass
|
||||
def reconnect_delay_set(self, *a, **kw): pass
|
||||
on_connect = None
|
||||
on_disconnect = None
|
||||
paho_client.Client = _Client
|
||||
paho_mqtt.client = paho_client
|
||||
paho_mod.mqtt = paho_mqtt
|
||||
|
||||
for name, mod in [
|
||||
("rclpy", rclpy),
|
||||
("rclpy.node", node_mod),
|
||||
("std_msgs", std_msgs_mod),
|
||||
("std_msgs.msg", std_msgs_msg),
|
||||
("paho", paho_mod),
|
||||
("paho.mqtt", paho_mqtt),
|
||||
("paho.mqtt.client", paho_client),
|
||||
]:
|
||||
sys.modules.setdefault(name, mod)
|
||||
|
||||
|
||||
_make_rclpy_stub()
|
||||
|
||||
from saltybot_phone.vesc_mqtt_relay_node import (
|
||||
_MQTT_VESC_LEFT,
|
||||
_MQTT_VESC_RIGHT,
|
||||
_MQTT_VESC_COMBINED,
|
||||
_extract_combined_payload,
|
||||
_extract_motor_payload,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# _extract_motor_payload
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestExtractMotorPayload:
|
||||
"""Covers field extraction and type coercion for per-motor JSON."""
|
||||
|
||||
def _sample(self, **overrides):
|
||||
base = {
|
||||
"can_id": 61,
|
||||
"rpm": 1500,
|
||||
"current_a": 12.34,
|
||||
"current_in_a": 10.0,
|
||||
"duty_cycle": 0.4500,
|
||||
"voltage_v": 25.20,
|
||||
"temp_fet_c": 45.5,
|
||||
"temp_motor_c": 62.0,
|
||||
"fault_code": 0,
|
||||
"fault_name": "NONE",
|
||||
"alive": True,
|
||||
"stamp": 1000.0,
|
||||
}
|
||||
base.update(overrides)
|
||||
return base
|
||||
|
||||
def test_required_keys_present(self):
|
||||
p = _extract_motor_payload(self._sample())
|
||||
for key in ("rpm", "current_a", "voltage_v", "temperature_c",
|
||||
"duty_cycle", "fault_code", "ts"):
|
||||
assert key in p, f"missing key: {key}"
|
||||
|
||||
def test_rpm_is_int(self):
|
||||
p = _extract_motor_payload(self._sample(rpm=1500))
|
||||
assert isinstance(p["rpm"], int)
|
||||
assert p["rpm"] == 1500
|
||||
|
||||
def test_temperature_maps_to_temp_fet_c(self):
|
||||
p = _extract_motor_payload(self._sample(temp_fet_c=55.5))
|
||||
assert p["temperature_c"] == 55.5
|
||||
|
||||
def test_voltage_v(self):
|
||||
p = _extract_motor_payload(self._sample(voltage_v=24.8))
|
||||
assert p["voltage_v"] == 24.8
|
||||
|
||||
def test_duty_cycle(self):
|
||||
p = _extract_motor_payload(self._sample(duty_cycle=0.75))
|
||||
assert p["duty_cycle"] == 0.75
|
||||
|
||||
def test_fault_code_zero(self):
|
||||
p = _extract_motor_payload(self._sample(fault_code=0))
|
||||
assert p["fault_code"] == 0
|
||||
|
||||
def test_fault_code_nonzero(self):
|
||||
p = _extract_motor_payload(self._sample(fault_code=3))
|
||||
assert p["fault_code"] == 3
|
||||
|
||||
def test_ts_from_stamp(self):
|
||||
p = _extract_motor_payload(self._sample(stamp=12345.678))
|
||||
assert p["ts"] == 12345.678
|
||||
|
||||
def test_negative_rpm(self):
|
||||
p = _extract_motor_payload(self._sample(rpm=-3000))
|
||||
assert p["rpm"] == -3000
|
||||
|
||||
def test_negative_current(self):
|
||||
p = _extract_motor_payload(self._sample(current_a=-5.0))
|
||||
assert p["current_a"] == -5.0
|
||||
|
||||
def test_negative_duty_cycle(self):
|
||||
p = _extract_motor_payload(self._sample(duty_cycle=-0.5))
|
||||
assert p["duty_cycle"] == -0.5
|
||||
|
||||
def test_missing_required_key_raises(self):
|
||||
bad = self._sample()
|
||||
del bad["rpm"]
|
||||
try:
|
||||
_extract_motor_payload(bad)
|
||||
assert False, "expected KeyError"
|
||||
except KeyError:
|
||||
pass
|
||||
|
||||
def test_missing_stamp_uses_current_time(self):
|
||||
data = self._sample()
|
||||
del data["stamp"]
|
||||
before = time.time()
|
||||
p = _extract_motor_payload(data)
|
||||
after = time.time()
|
||||
assert before <= p["ts"] <= after
|
||||
|
||||
def test_json_roundtrip(self):
|
||||
p = _extract_motor_payload(self._sample())
|
||||
raw = json.dumps(p)
|
||||
restored = json.loads(raw)
|
||||
assert restored["rpm"] == p["rpm"]
|
||||
assert restored["fault_code"] == p["fault_code"]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# _extract_combined_payload
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestExtractCombinedPayload:
|
||||
def _sample(self, **overrides):
|
||||
base = {
|
||||
"voltage_v": 25.2,
|
||||
"total_current_a": 18.5,
|
||||
"left_rpm": 1400,
|
||||
"right_rpm": 1420,
|
||||
"left_alive": True,
|
||||
"right_alive": True,
|
||||
"stamp": 2000.0,
|
||||
}
|
||||
base.update(overrides)
|
||||
return base
|
||||
|
||||
def test_required_keys_present(self):
|
||||
p = _extract_combined_payload(self._sample())
|
||||
for key in ("voltage_v", "total_current_a", "left_rpm", "right_rpm",
|
||||
"left_alive", "right_alive", "ts"):
|
||||
assert key in p, f"missing key: {key}"
|
||||
|
||||
def test_voltage_v(self):
|
||||
p = _extract_combined_payload(self._sample(voltage_v=24.0))
|
||||
assert p["voltage_v"] == 24.0
|
||||
|
||||
def test_total_current_a(self):
|
||||
p = _extract_combined_payload(self._sample(total_current_a=30.5))
|
||||
assert p["total_current_a"] == 30.5
|
||||
|
||||
def test_rpms_are_int(self):
|
||||
p = _extract_combined_payload(self._sample(left_rpm=1000, right_rpm=1050))
|
||||
assert isinstance(p["left_rpm"], int)
|
||||
assert isinstance(p["right_rpm"], int)
|
||||
|
||||
def test_alive_flags(self):
|
||||
p = _extract_combined_payload(self._sample(left_alive=True, right_alive=False))
|
||||
assert p["left_alive"] is True
|
||||
assert p["right_alive"] is False
|
||||
|
||||
def test_ts_from_stamp(self):
|
||||
p = _extract_combined_payload(self._sample(stamp=9999.1))
|
||||
assert p["ts"] == 9999.1
|
||||
|
||||
def test_missing_stamp_uses_current_time(self):
|
||||
data = self._sample()
|
||||
del data["stamp"]
|
||||
before = time.time()
|
||||
p = _extract_combined_payload(data)
|
||||
after = time.time()
|
||||
assert before <= p["ts"] <= after
|
||||
|
||||
def test_json_roundtrip(self):
|
||||
p = _extract_combined_payload(self._sample())
|
||||
raw = json.dumps(p)
|
||||
restored = json.loads(raw)
|
||||
assert restored["voltage_v"] == p["voltage_v"]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Rate-limit logic (isolated, no ROS2 / paho)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestRateLimit:
|
||||
"""
|
||||
Exercise the rate-limiting gate that lives inside VescMqttRelayNode._on_vesc.
|
||||
We test the guard condition directly without instantiating the ROS2 node.
|
||||
"""
|
||||
|
||||
def _make_gate(self, rate_hz: float):
|
||||
"""
|
||||
Return a stateful callable that mirrors the rate-limit check in the node.
|
||||
Returns True if a publish should proceed, False if the message is dropped.
|
||||
"""
|
||||
min_interval = 1.0 / rate_hz
|
||||
state = {"last": 0.0}
|
||||
|
||||
def gate(now: float) -> bool:
|
||||
if now - state["last"] < min_interval:
|
||||
return False
|
||||
state["last"] = now
|
||||
return True
|
||||
|
||||
return gate
|
||||
|
||||
def test_first_message_always_passes(self):
|
||||
gate = self._make_gate(5.0)
|
||||
assert gate(time.monotonic()) is True
|
||||
|
||||
def test_immediate_second_message_dropped(self):
|
||||
gate = self._make_gate(5.0)
|
||||
t = time.monotonic()
|
||||
gate(t)
|
||||
assert gate(t + 0.001) is False # 1 ms < 200 ms interval
|
||||
|
||||
def test_message_after_interval_passes(self):
|
||||
gate = self._make_gate(5.0)
|
||||
t = time.monotonic()
|
||||
gate(t)
|
||||
assert gate(t + 0.201) is True # 201 ms > 200 ms interval
|
||||
|
||||
def test_exactly_at_interval_dropped(self):
|
||||
gate = self._make_gate(5.0)
|
||||
t = time.monotonic()
|
||||
gate(t)
|
||||
# Exactly at the boundary is strictly less-than, so it's dropped
|
||||
assert gate(t + 0.2) is False
|
||||
|
||||
def test_10hz_rate(self):
|
||||
gate = self._make_gate(10.0)
|
||||
t = time.monotonic()
|
||||
gate(t)
|
||||
assert gate(t + 0.09) is False # 90 ms < 100 ms
|
||||
assert gate(t + 0.101) is True # 101 ms > 100 ms
|
||||
|
||||
def test_1hz_rate(self):
|
||||
gate = self._make_gate(1.0)
|
||||
t = time.monotonic()
|
||||
gate(t)
|
||||
assert gate(t + 0.999) is False
|
||||
assert gate(t + 1.001) is True
|
||||
|
||||
def test_multiple_topics_independent(self):
|
||||
gate_left = self._make_gate(5.0)
|
||||
gate_right = self._make_gate(5.0)
|
||||
t = time.monotonic()
|
||||
gate_left(t)
|
||||
gate_right(t)
|
||||
# left: drop, right: drop
|
||||
assert gate_left(t + 0.05) is False
|
||||
assert gate_right(t + 0.05) is False
|
||||
# advance only left past interval
|
||||
assert gate_left(t + 0.21) is True
|
||||
assert gate_right(t + 0.21) is True
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# MQTT topic constant checks
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class TestTopicConstants:
|
||||
def test_left_topic(self):
|
||||
assert _MQTT_VESC_LEFT == "saltybot/phone/vesc_left"
|
||||
|
||||
def test_right_topic(self):
|
||||
assert _MQTT_VESC_RIGHT == "saltybot/phone/vesc_right"
|
||||
|
||||
def test_combined_topic(self):
|
||||
assert _MQTT_VESC_COMBINED == "saltybot/phone/vesc_combined"
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import pytest
|
||||
pytest.main([__file__, "-v"])
|
||||
Loading…
x
Reference in New Issue
Block a user