ROS机器人竞赛Navigation导航包参数调试及程序修改经验

1、关闭恢复行为Recovery beheviors

   当, 全局规划失败（PLANNING_R）、局部规划失败（CONTROLLING_R）、长时间在小区域运动或陷入震荡（OSCILLATION_R ）时，Movebase会将当前的状态设置为CLEARING，车速设为为0，进入恢复行为，

   在竞速比赛中，当然不希望出现以上行为，因此，可以将恢复行为关掉，在move_base.cpp中搜索以下内容

state_ = CLEARING

   会得到3个搜索结果，分别对应以上的三种情况，分别点击该搜索结果，跳转到相关程序段

---

   将这三处相关程序注释掉，如下面三幅图所示，然后保存，catkin_make编译后,再规划的时候就会发现没有重规划的信息了，经过我的实际测试，没有再出现过调用重规划的情况。

---

---

2、hybrid astar全局规划器的使用（ompl库的安装）

   基于混合A*算法的全局路径规划器hybrid_astar_planner，需要用到开源运动规划库ompl，若之前没有装过，编译时则会遇到以下报错：

Make Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by “ompl” with any of
the following names:
omplConfig.cmake
ompl-config.cmake
Add the installation prefix of “ompl” to CMAKE_PREFIX_PATH or set

   解决方法，可以使用apt的方式进行安装ompl，如下指令所示（其中，melodic是对应的ROS版本，可根据实际情况修改）

sudo apt-get install ros-melodic-ompl*

   安装成功后，如下所示

   再次编译，即可成功编译，如下所示

---

   如果仅仅使用ompl运动规划库，通过apt的方式安装是没问题，若想阅读源码，或进行修改，则需要使用源码的方式安装：

   （1）通过以下网址，访问opml的官网，并点击Download the OMPL installation script下载源码包

https://ompl.kavrakilab.org/installation.html

   下载完成后，你将获得一个.sh文件，如下所示，

   在该目录下，右键打开终端，依次执行如下两条指令，就可以以源码的形式安装完成opml库了，此外，安装目录的/build/Release/bin下有很多生成的demo可执行文件，有兴趣的可以尝试运行使用。

chmod u+x install-ompl-ubuntu.sh
./install-ompl-ubuntu.sh

---

---

3、自动发送目标点

---

4、通过脚本运行多个launch文件

---

5、全局路径外扩策略

   在修改之前，我们必须对Costmap_2D产生的代价地图做一定的了解，使用激光雷达获取的环境地图信息，经过Costmap_2D功能包处理后转化为costmap代价地图。ROS的costmap通常采用grid网格形式。

   每个栅格都可以分为三个状态：

   （1）Occupied（有障碍，costmap_2d::LETHAL_OBSTACLE=254）

   （2）Free （ 无障碍，costmap_2d::FREE_SPACE=0）

   （3）Unknown Space（未知区域，costmap_2d::NO_INFORMATION=255）。

   所以规划所使用的环境地图的代价costs是由0~255的数字构成的，其中255表示未知区域，254表示致命的障碍，机器人的中心与该栅格的中心重合，机器人已经与障碍物发生了碰撞，253表示栅格中心位于机器人的内切轮廓内，机器人也发生了碰撞。128-252表示，网格中心位于机器人的外切圆与内切圆轮廓之间，此时机器人相当于靠在障碍物附近。0 ~1 27表示网格中心位于机器人外切圆之外，属于没有障碍物的空间。

   尽管，costs的取值范围为0~255，但常常仅用Occupied、Free、Unknown Space三种状态来区分

---

   方法一：修改expander.h中的，参数lethal_cost_的默认值，原默认值为253，在A算法进行点的扩展时，会将costs值大于lethal_cost_的栅格视为障碍物，所以可以将该值减小，当A算法在扩展与障碍物较近的点时，将该点也视为障碍物点，直接返回

 Expander(PotentialCalculator* p_calc, int nx, int ny) :
                unknown_(true), lethal_cost_(253), neutral_cost_(50), factor_(3.0), p_calc_(p_calc) {
            setSize(nx, ny);

  //障碍物点、未知点，跳过
    //lethal_cost_值在expander.h中设置为了253，
    if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION))
        return;

   经过理论分析，该方法较简单，但当局部地图漂移严重时（如下图所示），可能会出现找不到可行路径的情况，即当激光雷达的漂移比较大时，可能会导致无可行拓展点，从而找不到可行路径。

---

   方法二： 整体思路，改进A*算法的代价函数，增加与障碍物的距离代价，当栅格离障碍物较近时，给予附加的代价值b(n)。、

   比赛赛道的宽度大约是90cm，栅格精度是5cm，在地图不膨胀的情况下，每个方向检查8个栅格左右，即可使得赛道中心位置的代价最小，在global_planner包中将栅格间间距代价neutral_cost_视为50，因此，可以将b(n)取为(Detect_grid_radiu-k_ob+1）*neutral_cost_，其中Detect_grid_radiu是每个方向检测的栅格数，k_ob是在任意方向检测到障碍物时的栅格数，当检测至第Detect_grid_radiu个栅格仍然没有障碍物时，k_ob取Detect_grid_radiu，即此时b（n）=0

b(n)=(Detect_grid_radiu-k_ob+1）*neutral_cost_

//优化点5：计算与障碍物距离的附加代价b(n)，使得整体代价值变为 f=g+h+b
    
     int Detect_grid_radiu=20;   //朝上下左右四个方向各检查的栅格数量
     int neutral_cost_ob=20;   //代价扩大系数
     int cost_b=0;  //附加代价b(n),初始化为0；
     
     for (int check_i=1;check_i<=Detect_grid_radiu;check_i++)
     {
       if((costs[next_i+check_i]>=lethal_cost_)||(costs[next_i-check_i]>=lethal_cost_)||(costs[next_i+nx_*check_i]>=lethal_cost_)||(costs[next_i-nx_*check_i]>=lethal_cost_))
       {
         cost_b=(Detect_grid_radiu-check_i+1)*neutral_cost_ob*neutral_cost_;
         //ROS_INFO("check_i: %d", check_i);
         break;
       } 
     }

     //ROS_INFO("check_i  test");

    //将当前点及其代价f值，放到队列queue_中作为待选点，引入附加代价b(n)，使得整体代价值变为 f=g+h+b
    queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_ + cost_b));

---

6、多控制指令优先级配置

   比赛中，我们需要协调来自于多方的控制指令，比如，路径规划发布的控制指令、视觉发布的控制指令、其他算法发布的控制指令等，为其设置优先级，我们需要更改发送给下位机的cmd_vel话题，经过我们设定的优先级选择后，再发布给下位机，因此，我们创建一个新的节点cmd_vel2，它接收各个控制指令，并发出最终的控制指令,将发生给下位机的控制指令话题改为cmd_vel2。

sub_move_base = handle.subscribe("cmd_vel",1,&actuator::callback_move_base,this);

---

7、ROS同名功能包的优先级

   若在不同到的工作空间中，同时存在同名功能包，在执行是会选择优先级最高的那个执行，需要注意的是，优先级与.bashrc文件中设定的顺序是相反的，若.bashrc文件中设定如下：

source /opt/ros/noetic/setup.bash
source /home/gly/catkin_ws/devel/setup.bash

   在终端输入以下语句：

echo $ROS_PACKAGE_PATH

   终端的输出如下：

/home/gly/catkin_ws/src : /opt/ros/noetic/share

   即ROS在寻找某一功能包时会按照上述的目录顺序进行寻找。

---

8、速度较快时，里程计漂移过大问题

   实验场地地面为瓷砖（速度快时易打滑使得里程计误差较大），前进速度设置为大于1m/s，使用遥控器控制机器人从起点处按照比赛路线走一圈后，回到起点后的代价地图为：

   使用rqt_graph可以看出导航使用的定位信息是/odom_ekf，如下图所示（点击或拖拽可查看大图）：

   此时，/odom_ekf输出的位置信息如下图所示：发现x轴方向已经漂移了1.5m左右，y轴方向漂移了0.69m左右

   我们再来看一下，尚未使用的amcl输出的amcl_pose输出的信息，x轴和y轴分配漂移0.08m、0.09m，再去除跑完一圈后回到的起点与原起点可能存在较小的差别，可以认为这个漂移是可以接受的。

   所以，上述问题的一种解决思路是利用amcl_pose的信息去纠正odom_ekf的信息，amcl_pose的输出频率较低，所以不能直接使用amcl_pose来替代odom_ekf作为定位信息使用，一种较简单的组合思路是，当每获得一次amcl_pose的信息时就对odom_ekf进行一次修正。

---

9、轻舟机器人规划和视觉联合的思路

   （1）红绿灯

   视觉部分反馈给出小车当前距离红绿灯的距离和红绿灯的识别颜色，并以话题的形式发布出来，话题名为trafficLight，在qingzhou_bringup节点中会订阅这个话题

sub_trafficlight = handle.subscribe("trafficLight",1,&actuator::callback_trafficlight,this);

   并将接收到的数据通过回调函数，存放在currenttrafficlight中

void actuator::callback_trafficlight(const qingzhou_cloud::trafficlight::ConstPtr &msg){    
    currenttrafficlight = *msg;
    //printf("actuator-->trafficlight distanceX = %.2f,distanceY = %.2f,status = %d\n",currenttrafficlight.X,currenttrafficlight.Y,currenttrafficlight.trafficstatus);
}

   qingzhou_bringup节点会订阅mov_base调用路径规划发布出的运动控制指令cmd_vel

sub_move_base = handle.subscribe("cmd_vel",1,&actuator::callback_move_base,this);

   当接收到数据后，触发回调函数callback_move_base，在该回调函数中会将接收到的cmd_vel的线速度和角度控制指令分别存放在变量v和w中

   float v = msg->linear.x;                                                  
   float w = msg->angular.z;    

   此时，会进行判断，若之前存储在currenttrafficlight中当前小车与红绿灯的距离在设定的范围内，并且识别的红绿灯颜色为红色，则将v和w均置为0（相当于屏蔽掉了路径规划的控制指令），实现停车，等待红绿灯变为绿灯后，不满足上述条件，不再将v和w置零，按照路径规划发布的速度指令继续前行。

 if(currenttrafficlight.X < 1600 && currenttrafficlight.X > 100 && currenttrafficlight.trafficstatus == 2){
	  printf("actuator-->traffic X=%.2f\n",currenttrafficlight.X);
      v = 0;                                                                 
	  w = 0;
   //currenttrafficlight.trafficstatus = 0;
   }

   moveBaseControl.TargetSpeed = v*32/0.43;                                 
   moveBaseControl.TargetAngle = round(atan(w*CARL/v)*57.3);                
   moveBaseControl.TargetAngle+=60;  

---

   （2）S弯

   视觉部分会反馈出当前是否识别到S弯的车道线以及提取出的一些列车道线的中点坐标，并以话题的形式发布出来，话题名为roadLine，qingzhou_nav和qingzhou_bringup这两个节点都会订阅这个话题

 //一个订阅器，用于接收道路线信息。
sub_roadlinePoints = nh.subscribe("roadLine",1,&qingzhou_nav::callback_roadlinePoints,this); 

   都通过回调函数将信息保存在sRoadLinePointMsg中

void actuator::callback_roadlinePoints(const qingzhou_cloud::roadLine::ConstPtr &msg){
    sRoadLinePointMsg = *msg;
    //printf("actuator --> callback_roadlinePoints,linestatus = %d\n",sRoadLinePointMsg.lineStatus);
  }

   在qingzhou_nav节点的主循环中会判断sRoadLinePointMsg中存储的是否检测到车道线的标志位信息，若是，则利用sRoadLinePointMsg中储存的一些列车道线的中点坐标，然后使用纯跟踪算法来计算当前期望的舵机转角，并以话题roadLine_angle的形式发布出去，供qingzhou_bringup节点使用。

 // 如果检测到道路线状态为0x01，即检测到道路线，调用sLineControl函数计算目标角度，并发布道路线角度和导航状态。
    if(sRoadLinePointMsg.lineStatus == 0x01)
    {
	    //printf("success sroad-->linestatus = %d\n",sRoadLinePointMsg.lineStatus);
      float tagetAngle = sLineControl();
	    std_msgs::Float32 currentAngle;
	    currentAngle.data = tagetAngle;
	    pub_roadLine_angle.publish(currentAngle);

      nav_status.data = 3;   //S弯巡线时，导航状态为3
      pub_nav_status.publish(nav_status);
      continue;
    }

pub_roadLine_angle = nh.advertise<std_msgs::Float32>("roadLine_angle", 5 );  //一个发布器，用于发布S弯巡线的期望角度信息，

   在qingzhou_bringup节点中，在向下位机传输数据时，会判断sRoadLinePointMsg中存储的是否检测到车道线的标志位，若没有检测到车道线，则将路径规划计算得到的期望线速度和角速度经过转化后的数据，作为控制指令传输给下位机，若检测到车道线，则将订阅得到的qingzhou_nav发布的期望角度经过转换，以及设定的确定速度作为控制指令给下位机。以此，完成规划和识别控制指令的协调。

    if(sRoadLinePointMsg.lineStatus == 0)
    {                  
	    buf[3] = (int)moveBaseControl.TargetAngleDir;	    // 0-->go straight,0x10-->turn left,0x20-->turn right
	    buf[4] = (int)abs(moveBaseControl.TargetAngle);	    //targetangle 180-->0xb4
	    buf[5] = (int)abs(moveBaseControl.TargetSpeed);		//targetSpeed
	    buf[6] = (int)moveBaseControl.TargetModeSelect;		//0-->person control,1-->auto control
	    buf[7] = (int)moveBaseControl.TargetShiftPosition;  //0-->P stop;1-->R;2-->D.期望档位 停止/倒车/前进
    }
	else if(sRoadLinePointMsg.lineStatus == 1)
    {            
	    if(currentAngleMsg.data > 59){                      
	    	currentAngleMsg.data = 59;
	    }
	    if(currentAngleMsg.data < -59){
	    	currentAngleMsg.data = -59;
	    }
	    buf[3] = 0x10;                                      //与视觉程序对应
	    buf[4] = currentAngleMsg.data + 60;                 //期望转角+60 
	    buf[5] = 0.18*32/0.43;	                            //线速度
	    buf[6] = 1;                                         //自动模式
	    buf[7] = 2;	                                        //前进
    }

   上述程序中的currentAngleMsg来源于上面提到的qingzhou_nav发布的roadLine_angle。

 sub_roadLine_Angle = handle.subscribe("roadLine_angle",1,&actuator::callback_roadLineAngle,this);

void actuator::callback_roadLineAngle(const std_msgs::Float32::ConstPtr &msg){
    currentAngleMsg = *msg;
}

---

   （3）坡道

   （4）倒车入库

---

   注：这篇博客是去年6月份写的，本来想比赛完后，发出来，结果忘了…，还有两部分没写，尴尬是这个比赛没了…就不写了。。。。今天整理草稿箱，发现了这篇文章，正好今天是在CSDN创作四周年，正好我做智能车大概也有四年的时间。。所以还是把他发出来吧，纪念一下智能车生涯。

---