move_base源码解析

序言
好久没写博客了，最近把move_base源码包研究了一下，顺便以写博客的形式总结一下，同时里面掺杂了自己的心得体会，各路大神如果有不同的简介可以私下评论。

1 move_base_node.cpp
整个文件的开始以move_base_node.cpp开始。
从MoveBase::MoveBase的初始化开始。
as_作为维护动作服务的变量。

as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

以上代码重点在MoveBase::executeCb线程上。1.进入MoveBase::executeCb线程。** 2. 1.1首先执行

isQuaternionValid(move_base_goal->target_pose.pose.orientation)

用于判断目标点的格式对不对。
1.2然后执行

geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose)

用于将目标点的格式转换成通用格式
接下来发布目标点和设置发布频率。

current_goal_pub_.publish(goal);
ros::Rate r(controller_frequency_);

然后是更新cost_map,以及设置时间戳

if(shutdown_costmaps_){ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");planner_costmap_ros_->start();controller_costmap_ros_->start();}last_valid_control_ = ros::Time::now();last_valid_plan_ = ros::Time::now();last_oscillation_reset_ = ros::Time::now();planning_retries_ = 0;

然后就进入while循环函数，其中的if语句比较复杂。

//action的抢断函数
if(as_->isPreemptRequested())
{ //有没有新的目标点出现if(as_->isNewGoalAvailable()){//如果有新的目标点出现，就判断目标点的格式对不对，跟上面的那个函数一样if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");return;}goal = goalToGlobalFrame(new_goal.target_pose);recovery_index_ = 0;state_ = PLANNING;//唤醒路径规划线程lock.lock();planner_goal_ = goal;runPlanner_ = true;planner_cond_.notify_one();lock.unlock();//把目标发给可视化平台ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);current_goal_pub_.publish(goal);last_valid_control_ = ros::Time::now();last_valid_plan_ = ros::Time::now();last_oscillation_reset_ = ros::Time::now();planning_retries_ = 0;     } else//如果没接收到新目标{//14.重置状态,设置为抢占式任务resetState();ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");as_->setPreempted();return;}
//15.如果目标点的坐标系和全局地图的坐标系不相同，那就转换到世界坐标系，然后唤醒全局路径规划if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){goal = goalToGlobalFrame(goal);//we want to go back to the planning state for the next execution cyclerecovery_index_ = 0;state_ = PLANNING;//we have a new goal so make sure the planner is awakelock.lock();planner_goal_ = goal;runPlanner_ = true;planner_cond_.notify_one();lock.unlock();//publish the goal point to the visualizerROS_DEBUG_NAMED("move_base","The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y);current_goal_pub_.publish(goal);//make sure to reset our timeouts and counterslast_valid_control_ = ros::Time::now();last_valid_plan_ = ros::Time::now();last_oscillation_reset_ = ros::Time::now();planning_retries_ = 0;}//19. 到达目标点的真正工作，控制机器人进行跟随bool done = executeCycle(goal);
}

在executeCycle(goal)后面省略一些东西，主要是要进到这个函数了，判断机器人有没有到达目标位置。1.2.1 进入ecuteCycle(goal)函数```cpp
bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal)
{boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);//we need to be able to publish velocity commandsgeometry_msgs::Twist cmd_vel;geometry_msgs::PoseStamped global_pose;//得到机器人在世界坐标系下的位置getRobotPose(global_pose, planner_costmap_ros_);const geometry_msgs::PoseStamped& current_position = global_pose;//push the feedback outmove_base_msgs::MoveBaseFeedback feedback;feedback.base_position = current_position;as_->publishFeedback(feedback);//检测机器人当前的位置是否是在震荡if(distance(current_position, oscillation_pose_) >= oscillation_distance_){last_oscillation_reset_ = ros::Time::now();oscillation_pose_ = current_position;//if our last recovery was caused by oscillation, we want to reset the recovery index//如果上次的恢复是由振荡引起的，重置恢复指数if(recovery_trigger_ == OSCILLATION_R)recovery_index_ = 0;}//地图数据超时，即观测传感器数据不够新，停止机器人，返回false，其中publishZeroVelocity（）函数把geometry_msgs::Twist类型的cmd_vel设置为0并发布出去:if(!controller_costmap_ros_->isCurrent()){ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());publishZeroVelocity();return false;}if(new_global_plan_){//make sure to set the new plan flag to falsenew_global_plan_ = false;ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");//do a pointer swap under mutexstd::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);controller_plan_ = latest_plan_;latest_plan_ = temp_plan;lock.unlock();ROS_DEBUG_NAMED("move_base","pointers swapped!");//tc是格局部路径if(!tc_->setPlan(*controller_plan_)){//ABORT and SHUTDOWN COSTMAPSROS_ERROR("Failed to pass global plan to the controller, aborting.");resetState();//disable the planner threadlock.lock();runPlanner_ = false;lock.unlock();as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");return true;if(recovery_trigger_ == PLANNING_R)recovery_index_ = 0;}//接下来进入的是一个庞大的switch中，只是根据不同的状态值决定接下来该怎么做//然后判断move_base状态，一般默认状态或者接收到一个有效goal时是PLANNING，//在规划出全局路径后state_会由PLANNING变为CONTROLLING，如果规划失败则由PLANNING变为CLEARINGswitch(state_){//if we are in a planning state, then we'll attempt to make a plancase PLANNING://正在全局规划{boost::recursive_mutex::scoped_lock lock(planner_mutex_);runPlanner_ = true;planner_cond_.notify_one();}ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");break;//if we're controlling, we'll attempt to find valid velocity commandscase CONTROLLING://全局规划完了，就看看到没到终点，到了的话就关了规划ROS_DEBUG_NAMED("move_base","In controlling state.");//check to see if we've reached our goalif(tc_->isGoalReached()){ROS_DEBUG_NAMED("move_base","Goal reached!");resetState();//disable the planner threadboost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);runPlanner_ = false;//关闭规划lock.unlock();as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");return true;}//check for an oscillation condition//返回去继续看，如果超过震荡时间，停止机器人，设置清障标志位：震荡说明遇到了障碍，机器人在哪里晃if(oscillation_timeout_ > 0.0 &&last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now()){publishZeroVelocity();//发送速度为0state_ = CLEARING;recovery_trigger_ = OSCILLATION_R;}boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex()));//获取有效速度,如果获取成功，直接发布到cmd_vel：应该是局部速度，if(tc_->computeVelocityCommands(cmd_vel)){ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );last_valid_control_ = ros::Time::now();//make sure that we send the velocity command to the basevel_pub_.publish(cmd_vel);if(recovery_trigger_ == CONTROLLING_R)recovery_index_ = 0;}else //如果没有获取有效速度，该判断有没有超时{ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan.");ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_);//check if we've tried to find a valid control for longer than our time limit//则判断是否超过尝试时间，如果超时，则停止机器人，进入清障模式：if(ros::Time::now() > attempt_end){//we'll move into our obstacle clearing modepublishZeroVelocity();state_ = CLEARING;recovery_trigger_ = CONTROLLING_R;}else //如果没有超时，则再全局规划一个新的路径：{//otherwise, if we can't find a valid control, we'll go back to planninglast_valid_plan_ = ros::Time::now();planning_retries_ = 0;state_ = PLANNING;publishZeroVelocity();//enable the planner thread in case it isn't running on a clockboost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);runPlanner_ = true;planner_cond_.notify_one();lock.unlock();}}}break;//we'll try to clear out space with any user-provided recovery behaviorscase CLEARING://规划失败了ROS_DEBUG_NAMED("move_base","In clearing/recovery state");//we'll invoke whatever recovery behavior we're currently on if they're enabled//如果有可用恢复器，执行恢复动作，并设置状态为PLANNING：if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_+1, recovery_behaviors_.size());move_base_msgs::RecoveryStatus msg;msg.pose_stamped = current_position;msg.current_recovery_number = recovery_index_;msg.total_number_of_recoveries = recovery_behaviors_.size();msg.recovery_behavior_name =  recovery_behavior_names_[recovery_index_];recovery_status_pub_.publish(msg);recovery_behaviors_[recovery_index_]->runBehavior();//we at least want to give the robot some time to stop oscillating after executing the behaviorlast_oscillation_reset_ = ros::Time::now();//we'll check if the recovery behavior actually workedROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");last_valid_plan_ = ros::Time::now();planning_retries_ = 0;state_ = PLANNING;//update the index of the next recovery behavior that we'll tryrecovery_index_++;}else{//如果没有可用恢复器，结束动作，返回true：ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");//disable the planner threadboost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);runPlanner_ = false;lock.unlock();ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");if(recovery_trigger_ == CONTROLLING_R){ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");}else if(recovery_trigger_ == PLANNING_R){ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");}else if(recovery_trigger_ == OSCILLATION_R){ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");}resetState();return true;}break;default:ROS_ERROR("This case should never be reached, something is wrong, aborting");resetState();//disable the planner threadboost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);runPlanner_ = false;lock.unlock();as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");return true;
}

讲了这么多其实才讲完初始化move_base中的第一句。

接下来就是一些基本的参数配置的问题。比如用那种全局路径规划器，局部路径规划器。

ros::NodeHandle private_nh("~");ros::NodeHandle nh;//触发模式（三种模式：规划、控制、振荡）设置为“规划中”：recovery_trigger_ = PLANNING_R;//get some parameters that will be global to the move base node，参数设置std::string global_planner, local_planner;private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));private_nh.param("planner_frequency", planner_frequency_, 0.0);private_nh.param("controller_frequency", controller_frequency_, 20.0);private_nh.param("planner_patience", planner_patience_, 5.0);private_nh.param("controller_patience", controller_patience_, 15.0);private_nh.param("max_planning_retries", max_planning_retries_, -1);  // disabled by defaultprivate_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);private_nh.param("oscillation_distance", oscillation_distance_, 0.5);// parameters of make_plan serviceprivate_nh.param("make_plan_clear_costmap", make_plan_clear_costmap_, true);private_nh.param("make_plan_add_unreachable_goal", make_plan_add_unreachable_goal_, true);`

接下来设置了三个容器。

//为三种规划器设置内存缓冲区：（planner_plan_保存最新规划的路径，传递给latest_plan_，//然后latest_plan_通过executeCycle中传给controller_plan_）//planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();

然后是路径规划线程。

planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

2.1 接下来进这个线程里转转

void MoveBase::planThread()
{ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread...");ros::NodeHandle n;ros::Timer timer;bool wait_for_wake = false;//1. 创建递归锁boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);//干完前面的那些看也看不懂的东西后，进入while循环while(n.ok()){//检查是否需要路径规划，不需要的话就在这里循环了while(wait_for_wake || !runPlanner_){//if we should not be running the planner then suspend this threadROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");planner_cond_.wait(lock);wait_for_wake = false;}ros::Time start_time = ros::Time::now();geometry_msgs::PoseStamped temp_goal = planner_goal_;lock.unlock();ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");//路径规划planner_plan_->clear();//需要强调的是全局路径规划要进makeplan这个函数里面这里就不进去了bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);//如果获得了plan,则将其赋值给latest_plan_if(gotPlan){ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());//pointer swap the plans under mutex (the controller will pull from latest_plan_)std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;lock.lock();planner_plan_ = latest_plan_;latest_plan_ = temp_plan;last_valid_plan_ = ros::Time::now();planning_retries_ = 0;new_global_plan_ = true;ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");//目标的路径已经有了，就是控制过程的事情if(runPlanner_)state_ = CONTROLLING;if(planner_frequency_ <= 0)runPlanner_ = false;lock.unlock();}//5. 达到一定条件后停止路径规划,进入清障模式//如果没有规划出路径，并且处于PLANNING状态，则判断是否超过最大规划周期或者规划次数//如果是 则进入自转模式，否则应该会等待MoveBase::executeCycle的唤醒再次规划else if(state_==PLANNING){ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);lock.lock();planning_retries_++;//规划次数if(runPlanner_ &&(ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){//we'll move into our obstacle clearing modestate_ = CLEARING;runPlanner_ = false;  // proper solution for issue #523publishZeroVelocity();recovery_trigger_ = PLANNING_R;}lock.unlock();}if(planner_frequency_ > 0){ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();if (sleep_time > ros::Duration(0.0)){wait_for_wake = true;timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);}}}
}

3.咱们走完路径规划线程，回到move_base初始化.。接下来的工作是创建发布者

//创建发布者，话题名一个是cmd_vel，一个是cunrrent_goal，一个是goal：vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );ros::NodeHandle action_nh("move_base");ac题tion_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);

然后是设定跟rviz相关的目标点

//提供消息类型为geometry_msgs::PoseStamped的发送goals的接口，比如cb为MoveBase::goalCB，//在rviz中输入的目标点就是通过这个函数来响应的：ros::NodeHandle simple_nh("move_base_simple");goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

然后是costmap参数的设定，像地图膨胀层这些参数。

//设置costmap参数，技巧是把膨胀层设置为大于机器人的半径：private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);

接下来是设定全局规划器，局部规划器

//设置全局路径规划器，planner_costmap_ros_是costmap_2d::Costmap2DROS*类型的实例化指针planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);planner_costmap_ros_->pause();//initialize the global plannertry {planner_ = bgp_loader_.createInstance(global_planner);planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);} catch (const pluginlib::PluginlibException& ex) {ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());exit(1);}//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map//局部路径规划器controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);controller_costmap_ros_->pause();//create a local plannertry {tc_ = blp_loader_.createInstance(local_planner);ROS_INFO("Created local_planner %s", local_planner.c_str());tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);} catch (const pluginlib::PluginlibException& ex) {ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());exit(1);}

剩下的基本就差不多了。

//开始更新costmap：planner_costmap_ros_->start();controller_costmap_ros_->start();//advertise a service for getting a plan//全局规划make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);//advertise a service for clearing the costmaps//开始清除地图服务：其中，调用了MoveBase::clearCostmapsService（）函数，提供清除一次costmap的功能clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);//if we shutdown our costmaps when we're deactivated... we'll do that now//如果不小心关闭了costmap， 则停用：if(shutdown_costmaps_){ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");planner_costmap_ros_->stop();controller_costmap_ros_->stop();}//load any user specified recovery behaviors, and if that fails load the defaults//加载指定的恢复器，加载不出来则使用默认的，这里包括了找不到路自转360：if(!loadRecoveryBehaviors(private_nh)){loadDefaultRecoveryBehaviors();}//initially, we'll need to make a plan//导航过程基本结束，把状态初始化：state_ = PLANNING;//we'll start executing recovery behaviors at the beginning of our listrecovery_index_ = 0;//we're all set up now so we can start the action serveras_->start();//启动动态参数服务器：dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);dsrv_->setCallback(cb);}

4.以上有的地方也没有弄太明白，在摸索的时候也发现自己有很多地方还需要不断学习，至于不懂的小伙伴可以下面留言。同时也提供一个链接，觉得写的不错：[https://www.cnblogs.com/JuiceCat/p/13040552.html]

注：本文是巧克力～唯心原创文章。创作不易，引用的话请注明出处！！！

move_base源码解析相关推荐

谷歌BERT预训练源码解析（二）：模型构建
版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明. 本文链接:https://blog.csdn.net/weixin_39470744/arti ...
谷歌BERT预训练源码解析（三）：训练过程
目录前言源码解析主函数自定义模型遮蔽词预测下一句预测规范化数据集前言本部分介绍BERT训练过程,BERT模型训练过程是在自己的TPU上进行的,这部分我没做过研究所以不做深入探讨.BE ...
谷歌BERT预训练源码解析（一）：训练数据生成
目录预训练源码结构简介输入输出源码解析参数主函数创建训练实例下一句预测&实例生成随机遮蔽输出结果一览预训练源码结构简介关于BERT,简单来说,它是一个基于Transfo ...
Gin源码解析和例子——中间件（middleware）
在<Gin源码解析和例子--路由>一文中,我们已经初识中间件.本文将继续探讨这个技术.(转载请指明出于breaksoftware的csdn博客) Gin的中间件,本质是一个匿名回调函数.这 ...
Colly源码解析——结合例子分析底层实现
通过<Colly源码解析--框架>分析,我们可以知道Colly执行的主要流程.本文将结合http://go-colly.org上的例子分析一些高级设置的底层实现.(转载请指明出于break ...
libev源码解析——定时器监视器和组织形式
我们先看下定时器监视器的数据结构.(转载请指明出于breaksoftware的csdn博客) /* invoked after a specific time, repeatable (based o ...
libev源码解析——定时器原理
本文将回答<libev源码解析--I/O模型>中抛出的两个问题.(转载请指明出于breaksoftware的csdn博客) 对于问题1:为什么backend_poll函数需要指定超时?我们 ...
libev源码解析——I/O模型
在<libev源码解析--总览>一文中,我们介绍过,libev是一个基于事件的循环库.本文将介绍其和事件及循环之间的关系.(转载请指明出于breaksoftware的csdn博客) 目前i ...
libev源码解析——调度策略
在<libev源码解析--监视器(watcher)结构和组织形式>中介绍过,监视器分为[2,-2]区间5个等级的优先级.等级为2的监视器最高优,然后依次递减.不区分监视器类型和关联的文件描 ...

move_base源码解析

move_base源码解析相关推荐

最新文章

热门文章