Autoware实车测试记录（四）--全局及局部路径规划相关功能以及发送控制指令至底盘

经过前面的一系列工作，现在小车在地图中任意位置已经可以实现实时定位以及检测到障碍物以及追踪这些障碍物。下面的内容主要是继续完成下一步-路径规划，这其中包括了全局宏观上的路径规划（全局路径规划）以及在行走过程中对障碍物进行躲避（局部路径规划）。

Autoware关于路径规划的功能需要启动的如下：

其中OpenPlanner - Global Planning下的op_global_planner是全局路径规划。OpenPlanner - Local planning下的五个功能为局部路径规划部分。waypoint follower下的两个功能为向底盘下发控制指令的部分。

首先，在启动前文所讲的所有功能后，检查后台终端是否有检测到障碍物相关信息的输出，有输出的话代表前面的节点在正常工作，然后把前面的障碍物追踪节点（lidar_kf_contour_track）关闭，先进入OpenPlanner - Local planning下的op_common_params中修改避障参数，修改好后启动，再返回去启动障碍物追踪节点。

前期测试时发现后期怎么修改避障参数都没有用，后来看了一下功能包里面的README，发现障碍物参数被前期的障碍物追踪节点订阅，猜想可能先启动障碍物追踪模块会自动载入默认避障参数导致后面进行参数修改时无效。

 接下来首先启动全局路径规划部分，op_global_planner功能设置如下：

 测试时画的矢量地图及其简陋没有绘制车道线，故没有选择Lane Changing。Replanning、Smoothing、Rviz Goals比较好理解，分别对应着目标点重规划、平滑处理、可以通过Rviz给出目标点。

设置完成后启动这个节点，此时进入rviz（没打开的话可以在Autoware界面点击启动），在rviz中选择add，在弹出窗口中选择by topic，添加vector_map_center_ lines_rviz，添加后可以看到rviz中出现了路的中线。

 rviz中出现道路中线后，就可以进行全局路径规划了，在rviz上方，有一个2D Nav Goal，就是在前文定位部分给定初始位姿位置的旁边，使用它给定一个目标点，理论上地图中就会出现一条蓝色的规划路线：

 实际情况下，这一步我经常遇到如下几种情况：

1、大概率后台终端会出现下面的信息，并且没有全局路径生成。

Goal Found，LaneID：X，Distance：X   XXX······
Can‘t Generate Global Path for Start （X：···········

2、部分情况下全局路径规划反向规划，因为我的地图是一个环形。

同时生成全局路径规划并不是起点到给定终点的线段，反而是在制作矢量地图的时候所画的一整根直线一起被规划变为蓝色，且临近转弯处出现规划混乱的错误。

上面的这些情况出错原因暂时没有找到，猜想可能是为测试准备的矢量地图绘制的不严谨或者ndt节点这里有点问题，先挖个坑，有知道的欢迎点拨一二。

遇到的前两个问题，通常先给定目标点，再重新给位姿估计，因为这些报错都是初始点的信息出于某种原因不太正确，所以重新给一个初始位姿估计。虽然重新给位姿估计会使本来定位好的ndt又重新开始匹配，但是没有办法，多尝试几个初始位置，直到出现全局路径规划的蓝色线条出现就可以了。这只是个权宜之计，后期再返工重新研究这里，当前先把小车跑起来比较关键。

出现路径规划之后，可以开启局部路径规划的相关节点了，启动OpenPlanner - Local planning下的所有功能（五个），前期已经启动了参数部分，故只需要启动剩余四个。

其中对里面功能进行简单介绍：

op_common_params:

Horizon:视野范围（暂时不知道用途）

Plan Ditance:设置局部路径的长度，即局部路径从车体沿主路延伸有多长

Path Density:局部路径上相邻的两个轨迹点之间的距离，沿车道方向

Horizontal Density:不同局部路径之间的间距，垂直于车道方向

Rollouts Number:局部路径的数量

Max Velocity:最大速度值

Acceleration:加速度值

Follow Distance:沿轨迹多远开始检测追踪障碍物

Avoiding Distance:距离障碍物多远开始执行绕行

Avoidance LImit:停车距离，障碍物距离小于此值停车

Lateral & Longitudinal Safety:车安全框的尺寸，分别代表横向和纵向。

 op_trakectory_generator:

里面只有两个参数设置，具体含义见下图：

 余下的三个打开一看就好一般情况下参数默认即可。

启动所有局部路径功能后，回到rviz中，依旧是左下角add，弹出菜单选择by topic，依次添加下图中三个，在rviz中便会显示实时的局部路径规划。

 局部路径部分启动好后就可以启动控制节点了，控制功能为waypoint follower下的pure_pursuit和twist_filter两个部分。在这里参数我采用的默认值，启动两个节点后，rviz中会出现一个绿色的小球以及白色的预测轨迹线：

 至此理论上所有必需的功能都已经启动了，此时可以查看一下控制话题是否有数据输出，可以查看/twist_raw以及/twist_cmd是否正确下发数据，这两个话题分别对应着pure_pursuit和twist_filter。

由于我所使用的底盘接受的控制话题为/cmd_vel。所以还需要一个节点将Autoware的控制指令转化为/cmd_vel格式。/twist_raw的消息格式为 geometry_msgs/TwistStamped 。而/cmd_vel 的消息格式式为geometry_msgs/Twist。

转换功能包参考的博客：https://blog.youkuaiyun.com/m0_46259216/article/details/124478161

需要代码如下：

创建功能包：

mkdir -p catkin_ws/src
cd catkin_ws/src
catkin_creat_pkg vel_transform roscpp geometry_msgs std_msgs

转换功能源码：

#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Twist.h>
 
ros::Publisher twist_pub_;
ros::Subscriber  twist_sub_;
 
void twistCmdCallback(const geometry_msgs::TwistStampedConstPtr& msg)
{
    geometry_msgs::Twist twist = msg->twist;
    twist_pub_.publish(twist);
 
    ROS_INFO("Transform Success !");
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "twist_transform");
    ros::NodeHandle nh;
    twist_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    twist_sub_ = nh.subscribe<geometry_msgs::TwistStamped>("twist_raw", 1, twistCmdCallback);
 
    ros::spin();
    return 0;
}

 Cmakelists.txt：

# add_executable中源码的文件名根据自己命名的cpp修改

add_executable(twist_transform src/twist_transform.cpp)
 
target_link_libraries(twist_transform
  ${catkin_LIBRARIES}
)

这里建议雷达驱动、消息格式转换等功能包不要放在autoware工作空间下，单独创立一个工作空间集中存放管理，也方便修改调试。

到这里如果一切顺利小车应该会按照设定的程序开始运行了！

关于实测遇到的其他预料之外的错误将在下篇博客中展开叙述。