一 内容概括
本章的内容是完成车辆的定位信息和车道线/障碍物信息测试。
二 车辆定位
车辆的定位输出信息分为好多种,经纬度,地图坐标等。之前用gazebo尝试过向模型添加虚拟GPS,IMU(类似于carsim和prescan这种添加真值传感器,专注于PnC的算法调试),但是找了一圈资料都没有关于AutoWare如何实现添加虚拟各种传感器的,问GPT的解释是AutoWare并不是一个仿真软件,而是一个自动驾驶开源软件;如果想达到完整的仿真链路,需要借助Carla或者Autoware自己研发的AWSim来进行操作,如果有知道如何不依赖Carla等软件进行虚拟传感器配置的大佬可以请教一下。
由于缺乏GPS传感器信息,无法使用gnss给出的信号,在仿真中可用的定位topic为 /localization/kinematic_state,文档中说明此信号为map坐标系下的车辆位置
在仿真界面指定车辆位置:
在终端输入:
ros2 topic echo --once /localization/kinematic_state
输出为:
header:
stamp:
sec: 1746284535
nanosec: 882871433
frame_id: map
child_frame_id: base_link
pose:
pose:
position:
x: 3689.26025390625
y: 73756.3125
z: 19.136
orientation:
x: -0.00010767875107617374
y: 0.0004308663327187301
z: 0.24245542630812889
w: 0.9701624446520456
...
继续摆放车辆,然后输出位置:
header:
stamp:
sec: 1746284704
nanosec: 82771956
frame_id: map
child_frame_id: base_link
pose:
pose:
position:
x: 3737.746826171875
y: 73778.265625
z: 19.153
orientation:
x: 6.651958613775299e-05
y: -0.00021915722005075246
z: 0.2904404812315374
w: 0.9568930318521766
...
header:
stamp:
sec: 1746284779
nanosec: 432732456
frame_id: map
child_frame_id: base_link
pose:
pose:
position:
x: 3761.676025390625
y: 73747.484375
z: 19.294
orientation:
x: -0.003388168096615101
y: -0.005649922382935947
z: -0.5142853167919871
w: 0.857593850039855
...
header:
stamp:
sec: 1746284869
nanosec: 7800724
frame_id: map
child_frame_id: base_link
pose:
pose:
position:
x: 3714.81494140625
y: 73719.5703125
z: 19.3398
orientation:
x: 0.0004987544309335415
y: 0.0001492396517919181
z: -0.958030339037751
w: 0.2866663538937835
...
用python画出这四个点的位置和heading,代码在图下,用四元数计算heading
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as R
import numpy as np
poses = [
{
"position": (3689.2603, 73756.3125),
"orientation": (-0.0001076788, 0.0004308663, 0.2424554263, 0.9701624447),
},
{
"position": (3737.7468, 73778.2656),
"orientation": (6.6519586e-05, -0.0002191572, 0.2904404812, 0.9568930319),
},
{
"position": (3761.6760, 73747.4844),
"orientation": (-0.0033881681, -0.0056499224, -0.5142853168, 0.8575938500),
},
{
"position": (3714.8149, 73719.5703),
"orientation": (0.0004987544, 0.0001492397, -0.9580303390, 0.2866663539),
},
]
x_list = [p["position"][0] for p in poses]
y_list = [p["position"][1] for p in poses]
plt.figure(figsize=(8, 8))
plt.plot(x_list, y_list, "k--", label="Trajectory")
for i, pose in enumerate(poses):
x, y = pose["position"]
quat = pose["orientation"]
yaw = R.from_quat(quat).as_euler("xyz")[2]
dx = np.cos(yaw) * 5
dy = np.sin(yaw) * 5
plt.plot(x, y, "ko")
plt.arrow(x, y, dx, dy, head_width=1, head_length=2, fc="blue", ec="blue")
plt.text(x, y, f"P{i+1}", fontsize=9, ha="right")
plt.axis("equal")
plt.grid(True)
plt.title("Vehicle Trajectory with Heading")
plt.xlabel("X")
plt.ylabel("Y")
plt.legend()
plt.show()
定位测试完成,能拿到自车的位置信息了,但是按理来说应该是经纬度的。这个后续看看再调一调,理想情况是可以的,因为现在拿到了地图坐标系下的坐标,而且我们有地图的信息,是可以推算出来的。
三 车道线/障碍物信息
一开始想着AutoWare会有虚拟的传感器或者就是topic来完成这个工作(对标PreScan的车道线传感器),但是很不幸,没有。查看了下demo的算法流程,是根据定位信息在地图中查找所在车道进而实现车道保持的。。。
(未完待续)