Compare commits
2 Commits
e2315358bf
..
main
| Author | SHA1 | Date | |
|---|---|---|---|
| 3973833fbb | |||
| 1799a5bb1b |
@@ -16,8 +16,22 @@ platform:
|
|||||||
wheelbase: 0.6 # m (= 기존 vehicle_mkmini.yaml cmd_vel_to_*.wheelbase)
|
wheelbase: 0.6 # m (= 기존 vehicle_mkmini.yaml cmd_vel_to_*.wheelbase)
|
||||||
max_steering_angle: 0.5 # rad
|
max_steering_angle: 0.5 # rad
|
||||||
max_speed: 0.6 # m/s (sim 상한; 실차 안전상한은 vehicle_driver 프로파일)
|
max_speed: 0.6 # m/s (sim 상한; 실차 안전상한은 vehicle_driver 프로파일)
|
||||||
track_width: # m — 미측정 (현재 odom/TF 미사용). 측정 후 기입
|
track_width: 0.51 # m (실측 휠 중심 간 거리)
|
||||||
wheel_radius: # 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 는 이름→노드만)
|
# 드라이버 선택 (선택 source = 이 데이터. registry.py 는 이름→노드만)
|
||||||
default_vehicle_driver: yhs_can # 실차 (sim_mode=false)
|
default_vehicle_driver: yhs_can # 실차 (sim_mode=false)
|
||||||
|
|||||||
@@ -12,5 +12,6 @@ vehicle_driver:
|
|||||||
input_topic: /cmd_vel
|
input_topic: /cmd_vel
|
||||||
output_topic: /tod_cmd
|
output_topic: /tod_cmd
|
||||||
angular_deadband: 0.06
|
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
|
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()
|
||||||
Reference in New Issue
Block a user