diff --git a/src/vehicle_pkg/config/vehicle_drivers/vehicle_driver_yhs_can.yaml b/src/vehicle_pkg/config/vehicle_drivers/vehicle_driver_yhs_can.yaml index 5f311b0..eedd35d 100644 --- a/src/vehicle_pkg/config/vehicle_drivers/vehicle_driver_yhs_can.yaml +++ b/src/vehicle_pkg/config/vehicle_drivers/vehicle_driver_yhs_can.yaml @@ -12,5 +12,6 @@ vehicle_driver: input_topic: /cmd_vel output_topic: /tod_cmd angular_deadband: 0.06 - min_speed_for_steer: 0.15 + # Allow the viewer's 0.1 m/s manual control without increasing its speed cap. + min_speed_for_steer: 0.05 max_speed: 0.3 diff --git a/src/vehicle_pkg/test/test_manual_steering.py b/src/vehicle_pkg/test/test_manual_steering.py new file mode 100644 index 0000000..56512db --- /dev/null +++ b/src/vehicle_pkg/test/test_manual_steering.py @@ -0,0 +1,53 @@ +"""Exercise the real TOD conversion without ROS or connected hardware.""" +import ast +import math +from pathlib import Path +from types import SimpleNamespace +import unittest + +import yaml + + +class ManualSteeringTest(unittest.TestCase): + def setUp(self): + root = Path(__file__).resolve().parents[1] + source = ast.parse((root / "vehicle_pkg/cmd_vel_to_tod.py").read_text()) + clamp = next(item for item in source.body if isinstance(item, ast.FunctionDef) and item.name == "clamp") + adapter = next(item for item in source.body if isinstance(item, ast.ClassDef) and item.name == "CmdVelToTod") + convert = next(item for item in adapter.body if isinstance(item, ast.FunctionDef) and item.name == "_on_cmd_vel") + scope = {"math": math, "Twist": SimpleNamespace, "Float64MultiArray": SimpleNamespace} + exec(compile(ast.Module(body=[clamp, convert], type_ignores=[]), "tod-test", "exec"), scope) + self.convert = scope["_on_cmd_vel"] + driver = yaml.safe_load((root / "config/vehicle_drivers/vehicle_driver_yhs_can.yaml").read_text())["vehicle_driver"]["adapter_params"] + platform = yaml.safe_load((root / "config/platforms/platform_mkmini.yaml").read_text())["platform"] + self.messages = [] + self.node = SimpleNamespace( + max_speed=driver["max_speed"], + min_spd_steer=driver["min_speed_for_steer"], + angular_db=driver["angular_deadband"], + wheelbase=platform["wheelbase"], + max_steer_rad=platform["max_steering_angle"], + _pub=SimpleNamespace(publish=lambda msg: self.messages.append(msg.data)), + ) + + def command(self, linear, angular): + self.convert(self.node, SimpleNamespace(linear=SimpleNamespace(x=linear), angular=SimpleNamespace(z=angular))) + return self.messages[-1] + + def test_low_speed_forward_and_reverse_steering(self): + for linear, gear in [(0.1, 4.0), (-0.1, 2.0)]: + for angular in [-0.08, 0.08]: + speed, _, steering, actual_gear = self.command(linear, angular) + self.assertEqual(speed, 0.1) + self.assertEqual(actual_gear, gear) + self.assertGreater(steering * angular, 0) + self.assertLessEqual(abs(steering), math.degrees(self.node.max_steer_rad)) + + def test_stop_and_steering_only_do_not_drive(self): + for angular in [0, 0.08, -0.08]: + speed, _, steering, _ = self.command(0, angular) + self.assertEqual((speed, steering), (0, 0)) + + +if __name__ == "__main__": + unittest.main()