最近,使用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 pausedif (stopped_){// if we're stopped we need to re-subscribe to topicsfor (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 runros::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 posegeometry_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 节点时,概率性会出现全局和局部代价地图不加载的问题,就可以得到解决。