广西北海联友建设网站管理,四川平台网站建设哪里有,站酷设计网站官网入口下载,个人网站建设的小清新图片最近#xff0c;使用ROS机器人#xff0c;在启动move_base 节点时#xff0c;概率性会出现全局和局部代价地图不加载的问题#xff0c;此时#xff0c;发布目标点也无法启动路径规划。而且该问题有时候出现概率很低#xff0c;比如启动10次#xff0c;会有1次发送该情况… 最近使用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_ptrLayer *plugins layered_costmap_-getPlugins();ROS_INFO(c2);// check if were stopped or just pausedif (stopped_){// if were stopped we need to re-subscribe to topicsfor (vectorboost::shared_ptrLayer ::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 节点时概率性会出现全局和局部代价地图不加载的问题就可以得到解决。
