Compare commits

...

2 Commits

Author SHA1 Message Date
chyoo 3973833fbb fix(mkmini): allow steering at low manual control speeds 2026-09-23 11:59:15 +09:00
chyoo 1799a5bb1b feat(nav2): add mkmini lidar mount profile 2026-08-12 10:17:22 +09:00
3 changed files with 71 additions and 3 deletions
@@ -16,8 +16,22 @@ platform:
wheelbase: 0.6 # m (= 기존 vehicle_mkmini.yaml cmd_vel_to_*.wheelbase)
max_steering_angle: 0.5 # rad
max_speed: 0.6 # m/s (sim 상한; 실차 안전상한은 vehicle_driver 프로파일)
track_width: # m — 미측정 (현재 odom/TF 미사용). 측정 후 기입
wheel_radius: # m — 미측정 (현재 미사용)
track_width: 0.51 # m (실측 휠 중심 간 거리)
wheel_radius: 0.12 # m (실측)
wheel_width: 0.05 # m (실측)
front_overhang: 0.12 # m (실측)
rear_overhang: 0.12 # m (실측)
left_overhang: 0.05 # m (실측)
right_overhang: 0.05 # m (실측)
lidar_mount:
x: 0.15 # m, base_link 기준
y: 0.0
z: 0.5
roll: 0.0
pitch: 0.0
yaw: 0.0
frame_id: base_link
child_frame_id: os_lidar
# 드라이버 선택 (선택 source = 이 데이터. registry.py 는 이름→노드만)
default_vehicle_driver: yhs_can # 실차 (sim_mode=false)
@@ -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
@@ -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()