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>
在/opt/Autoware下这个库已经有了，所以在adehome/AutowareAuto/src/planning/costmap_generator/package.xml 中直接增加依赖
<depend>grid_map_cv</depend>
1.1.2 velodyne_nodes 编译出错，仿真中用不到，屏蔽掉
colcon build --packages-ignore velodyne_nodes
编译完成
尝试从本地启动
第一次：rviz可以起来，但是没有加载出地图
原因 ：地图文件在/opt 下，需要修改launch 文件
修改 adehome/AutowareAuto/src/launch/avp_sim.launch.py 中的map_pcd_file和map_yaml_file的文件参数如下：
map_pcd_file = "/opt/AutowareAuto/share/autoware_demos/data/autonomoustuff_parking_lot_lgsvl.pcd"
map_yaml_file = "/opt/AutowareAuto/share/autoware_demos/data/autonomoustuff_parking_lot_lgsvl.yaml"
第二次启动：地图以及各可以出来，但是车辆model没有
修改 adehome/AutowareAuto/src/launch/avp_sim.launch.py 中的urdf_path文件参数如下：
urdf_path = "/opt/AutowareAuto/share/lexus_rx_450h_description/urdf/lexus_rx_450h.urdf"
最终发现车辆模型没有出来，且车辆不能运动，引起决定，将launch文件改为两部分，一部分启动自定义的节点，一部分启动/opt下已经生成的节点
上面编译有问题的节点全部使用/opt下的节点，还有 地图和urdf加载的节点，剩下的节点使用 自己编译完成的。
再次尝试，车辆可以动了，一切正常，车辆模型还是发不出来再修改下
把rviz启动改成启动/opt下的就可以了，应该是配置文件的问题，先不管了
基本ok,可以进行定制化开发！
下面针对感知boundingbox会入侵可行使区域进行优化：
(1) 确定被分类完的话题 /perception/points_clustered
节点名称/perception/euclidean_cluster_cloud_node
输入的话题为/points_nonground
参数文件：
/AutowareAuto/src/autoware_auto_launch/param/euclidean_cluster_.param.yaml
为了调试方便，将这个节点的启动单独放一个launch：
通过调节参数文件中的
min_cluster_threshold_m
max_cluster_threshold_m : 可以调节点云聚类，从而减小比较大的boundingbox入侵可行驶区域
此处的修改目的很简单，不要求感知的结果正确，只要求感知生成的障碍物不会偏大，而入侵了可行使区域。因此暴力的将上面两个参数修改为0.2 和0.8
2. 路径规划
在对感知进行修改后，车辆还会出现碰撞而卡住的情况。但是并非是撞到东西，而是进入了地图的不可行使区域，接下来主要解决以下几个问题：
1.车辆的卡住问题：
2.车辆不能倒车的问题：
3，车辆卡住后不能手动控制的问题：（人工接管）
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
