ROS机器人启动move base时代价地图概率性无法加载的原因及解决方法

最近，使用ROS机器人，在启动move_base 节点时，概率性会出现全局和局部代价地图不加载的问题，此时，发布目标点也无法启动路径规划。而且该问题有时候出现概率很低，比如启动10次，会有1次发送该情况，有时候概率又比较高，运气最差的一次，启动了8次才正常启动。

上图中，是正常情况下，rviz显示的局部代价地图，在确保rviz已经配置了显示局部代价地图后，上述异常情况下，该局部代价地图不会显示。此时，rviz的局部代价地图的map配置中会显示No map received，通过rostopic echo在终端打印局部代价地图信息，会发现该消息没有被发布出来。

在比赛过程中，若遇到这个问题，纯纯搞心态，初赛中，8分钟的比赛时间，卡在代价地图无法正常加载浪费了至少1分钟，所以还是下定决心要刨根问底，解决这个隐患。

为了找到异常启动 move_base 节点时程序是否卡在了某个地方，从而导致代价地图没有加载出来，我先后在move_base、costmap_2d、global_planner功能包的初始化函数（构造函数）以及调用的相关函数编写打印了大量的流程运行信息，用来分析程序卡在了什么地方。

在这里插入图片描述

–

经过层层套娃式深入探究和分析，最终确定上述异常的代价地图为正常加载是卡在了位于move_base构造函数中调用的以下函数中

planner_costmap_ros_->start();

planner_costmap_ros_中的start()函数具体程序位于costmap_2d_ros.cpp中，我添加了标志信息后的程序如下所示：

void Costmap2DROS::start()
{
    
    

  
  ROS_INFO("c1");


  std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();

  ROS_INFO("c2");

  // check if we're stopped or just paused
  if (stopped_)
  {
    
    
    // if we're stopped we need to re-subscribe to topics
    for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
        ++plugin)
    {
    
    
      (*plugin)->activate();
    }
    stopped_ = false;
  }
  stop_updates_ = false;

  ROS_INFO("c3");

  // block until the costmap is re-initialized.. meaning one update cycle has run
  ros::Rate r(100.0);

  ROS_INFO("initialized_: %d", initialized_);

  while (ros::ok() && !initialized_)
    r.sleep();

  ROS_INFO("c4");
}

上述异常导致此处调用start函数时的initialized_值为0，从而卡在了start函数的while循环中，从而使得move_base中planner_costmap_ros_->start()语句后的初始化部分未能执行，从而导致代价地图未能加载。

正常情况下，调用start函数时initialized_值为1。后来我深入探究了为什么有时候程序运行到planner_costmap_ros_->start()语句时，initialized_值为1（正常情况），有时候initialized_值为0（异常情况）。

经过层层套娃式分析，最终锁定到了costmap_2d_ros.cpp中的void Costmap2DROS::updateMap()函数中的layered_costmap_->updateMap(x, y, yaw);函数中

void Costmap2DROS::updateMap()
{
    
    

  ROS_INFO("true 490");

  if (!stop_updates_)
  {
    
    

    ROS_INFO("true 495");

    // get global pose
    geometry_msgs::PoseStamped pose;
    if (getRobotPose (pose))
    {
    
    
      double x = pose.pose.position.x,
             y = pose.pose.position.y,
             yaw = tf2::getYaw(pose.pose.orientation);


      ROS_INFO("up1");

      layered_costmap_->updateMap(x, y, yaw);

      ROS_INFO("up2");

      geometry_msgs::PolygonStamped footprint;
      footprint.header.frame_id = global_frame_;
      footprint.header.stamp = ros::Time::now();

      ROS_INFO("up3");

      transformFootprint(x, y, yaw, padded_footprint_, footprint);

      ROS_INFO("up4");

      footprint_pub_.publish(footprint);

      ROS_INFO("up5");

      initialized_ = true;
      ROS_INFO("true 505");
    }
  }
}

根本原因是正常情况下和异常情况下layered_costmap的updateMap(x, y, yaw)函数执行速度不同，导致了这个将initialized_值置为1的线程与另一个通过planner_costmap_ros_->pause()将initialized_值置为0的线程在正常和异常情况下的执行顺序不同，正常情况下另一个将initialized_值置为0的线程先执行，然后将其值置为1的线程后执行，异常情况下则相反，此时导致本线程程序卡在start函数，而不能正常运行。

至于该问题的解决方法，我编写了一个Costmap2DROS类的成员函数force_start()，在该函数中将initialized_的值强制置为1。然后在move_base.cpp文件中执行planner_costmap_ros_->start();之前，先执行该planner_costmap_ros_->force_start();语句，将initialized_的值强制置为1，此时，便不会出现上述程序卡在start()函数的情况。

planner_costmap_ros_->force_start();

添加在costmap_2d_ros.cpp文件中的force_start()函数的具体程序如下：

void Costmap2DROS::force_start()
{
    
    
  initialized_ = true;

  ROS_INFO("initialized_ force_start");

}

添加在costmap_2d_ros.h文件中Costmap2DROS类中的force_start()函数的声明如下：

  /**
   * @brief  initialized_ force_start
   */
  void force_start();

至此，经过以上修改，在启动move_base 节点时，概率性会出现全局和局部代价地图不加载的问题，就可以得到解决。

ROS机器人启动move base时代价地图概率性无法加载的原因及解决方法

猜你喜欢