Autoware.u学习历程（二）

Autoware.u学习之车辆定位测试

原创已于 2025-05-05 22:52:09 修改 · 561 阅读

7 ·

CC 4.0 BY-SA版权

文章标签：

#c++ #自动驾驶 #学习 #AutoWare

于 2025-05-03 23:12:48 首次发布

一内容概括

本章的内容是完成车辆的定位信息和车道线/障碍物信息测试。

二车辆定位

车辆的定位输出信息分为好多种，经纬度，地图坐标等。之前用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()

定位测试完成，能拿到自车的位置信息了，但是按理来说应该是经纬度的。这个后续看看再调一调，理想情况是可以的，因为现在拿到了地图坐标系下的坐标，而且我们有地图的信息，是可以推算出来的。