Autoware.Auto avp仿真详解

小鹿的先生

已于 2022-05-24 14:59:58 修改

阅读量1.3k

点赞数 1

文章标签： python 自动驾驶开发语言

于 2022-05-18 16:37:40 首次发布

本文链接：https://blog.csdn.net/luochenzhicheng/article/details/124734510

版权

1.定位

定位节点启动的是

    ndt_localizer = Node(
        package='ndt_nodes',
        executable='p2d_ndt_localizer_exe',
        namespace='localization',
        name='p2d_ndt_localizer_node',
        parameters=[
            LaunchConfiguration('ndt_localizer_param_file'),
            # Use preset initial pose, if using a preset simulation
            {'load_initial_pose_from_parameters': LaunchConfiguration('with_lgsvl')},
        ],
        remappings=[
            ("points_in", "/lidars/points_fused_downsampled"),
            ("observation_republish", "/lidars/points_fused_viz"),
        ]
    )

在仿真系统中ros2 node info /localization/p2d_ndt_localizer_node:

其定位结果输出的是话题是:

/localization/ndt_pose: geometry_msgs/msg/PoseWithCovarianceStamped

frame_id: map

也就是车辆在地图上的位姿

同时也向话题/tf 发送消息：

综上，在仿真中启用的定位节点是ndt_nodes\

出现的问题，在发送定位状态时，将四元数转换成欧拉角，ros2(foxy) 和 python 和ros1 有了区别，折腾了半天，解决方案如下：

解决方案有两个：

1.安装官方库

sudo apt install ros-foxy-tf-transformations
sudo pip3 install transforms3d

import tf_transformations
q = tf_transformations.quaternion_from_euler(r, p, y)
r, p, y = tf_transformations.euler_from_quaternion(quaternion)

2. 自己写个函数

import numpy as np

def euler_from_quaternion(quaternion):
    """
    Converts quaternion (w in last place) to euler roll, pitch, yaw
    quaternion = [x, y, z, w]
    Bellow should be replaced when porting for ROS 2 Python tf_conversions is done.
    """
    x = quaternion.x
    y = quaternion.y
    z = quaternion.z
    w = quaternion.w

    sinr_cosp = 2 * (w * x + y * z)
    cosr_cosp = 1 - 2 * (x * x + y * y)
    roll = np.arctan2(sinr_cosp, cosr_cosp)

    sinp = 2 * (w * y - z * x)
    pitch = np.arcsin(sinp)

    siny_cosp = 2 * (w * z + x * y)
    cosy_cosp = 1 - 2 * (y * y + z * z)
    yaw = np.arctan2(siny_cosp, cosy_cosp)

    return roll, pitch, yaw

上面两个解决方案都参考下面这个链接

https://gist.github.com/salmagro/2e698ad4fbf9dae40244769c5ab74434

仿真启动的核心launch文件是Autoware/src/autoware_demos/launch.avp_core.launch.py

首先对感知的结果进行解析学习。

感知结果，用于rviz显示的话题：

/perception/lidar_bounding_boxes_viz

1.感知：

avp 仿真中用的是简单的欧几里德聚类将点云进行聚类并输出boundingbox,但是存在一定问题，会出现将大片的点云聚类为一个障碍物，输出的boundingbox 会入侵到可行使区域导致车辆停止，需要优化。

为了便于修改，计划使用ade_home下的源码进行编译然后启动，方便自己修改各种需求。

编译问题

1.1 costmap_generator_nodes 编译错误

1.1.1找不到 <grid_map_cv/grid_map_cv.hpp> 和<grid_map_ros/grid_map_ros.hpp>