标签:accept 并且 更新 string opp wait std oop on()
零、前言
此前写了一篇对move_base的解析,号称全网最全,因为把整个包都加了注释hhh。
但是那篇文章由于是以源代码+注释的形式呈现,存在诸多问题,比如:没有按照逻辑顺序解析(而阅读代码的正确步骤是 按照程序执行顺序一步一步看),没有深挖里面的函数而更多的是对本框架的理解,也没有把各个函数之间通用的变量讲清楚(比如:move_base的状态实际上是贯穿整个规划过程的、各个标志位在不同函数之间都是有变化的),等等。
本篇博文在此前那篇的基础上,按照程序执行顺序一步一步进行讲解,并且在一定程度上进行了深挖(譬如,与其他包是怎么关联的),也在程序讲解时 把头文件中列出的类成员的含义进行了说明。
个人可能会有错误,欢迎朋友们批评指正,并私信我取得联系,在此感谢。
话不多说,开始解析——
一、入口
入口为文件move_base_node.cpp,声明了move_base::MoveBase对象move_base,传入参数为tf2_ros::Buffer& tf 。声明的时候,便进入了构造函数。但是在看构造函数前,我们先来看一下在move_base这个命名空间中都有哪些数据。
二、头文件-命名空间
进入入口的命名空间,命名空间为move_base。接下来挨个介绍一下命名空间move_base的内容:
1、声明server端,消息类型是move_base_msgs::MoveBaseAction
1 typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
2、枚举movebase状态表示
1 enum MoveBaseState { 2 PLANNING, 3 CONTROLLING, 4 CLEARING 5 };
3、枚举,触发恢复模式
1 enum RecoveryTrigger 2 { 3 PLANNING_R, 4 CONTROLLING_R, 5 OSCILLATION_R 6 };
4、MoveBase类,使用actionlib::ActionServer接口,该接口将robot移动到目标位置
1 class MoveBase{ } //下面详细展开
三、头文件-MoveBase类
在move_base命名空间中,最重要的就是MoveBase类了,在(一)中提到的入口,实际上就是构造函数。在这里,先罗列一下类中有哪些成员,然后在(四)中再详细解释各个函数成员与数据成员的关系与执行顺序。
1、构造函数,传入的参数是tf
MoveBase(tf2_ros::Buffer& tf);
2、析构函数
virtual ~MoveBase();
3、控制闭环、全局规划、 到达目标返回true,没有到达返回false
bool executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan);
4、清除costmap
bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
5、当action不活跃时,调用此函数,返回plan
bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
6、新的全局规划,goal 规划的目标点,plan 规划
bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
7、从参数服务器加载导航恢复行为
bool loadRecoveryBehaviors(ros::NodeHandle node);
8、加载默认导航恢复行为
void loadDefaultRecoveryBehaviors();
9、清除机器人局部规划框的障碍物,size_x 局部规划框的长x, size_y 局部规划框的宽y
void clearCostmapWindows(double size_x, double size_y);
10、发布速度为0的指令
void publishZeroVelocity();
11、重置move_base action的状态,设置速度为0
void resetState();
12、周期性地唤醒规划器
void wakePlanner(const ros::TimerEvent& event);
13、其它函数
1 void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal); 2 3 void planThread(); 4 5 void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal); 6 7 bool isQuaternionValid(const geometry_msgs::Quaternion& q); 8 9 bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap); 10 11 double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2); 12 13 geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
14、数据成员
1 tf2_ros::Buffer& tf_; 2 3 MoveBaseActionServer* as_; //就是actionlib的server端 4 5 boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;//局部规划器,加载并创建实例后的指针 6 costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;//costmap的实例化指针 7 8 boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;//全局规划器,加载并创建实例后的指针 9 std::string robot_base_frame_, global_frame_; 10 11 std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;//可能是出错后的恢复 12 unsigned int recovery_index_; 13 14 geometry_msgs::PoseStamped global_pose_; 15 double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_; 16 double planner_patience_, controller_patience_; 17 int32_t max_planning_retries_; 18 uint32_t planning_retries_; 19 double conservative_reset_dist_, clearing_radius_; 20 ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_; 21 ros::Subscriber goal_sub_; 22 ros::ServiceServer make_plan_srv_, clear_costmaps_srv_; 23 bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_; 24 double oscillation_timeout_, oscillation_distance_; 25 26 MoveBaseState state_; 27 RecoveryTrigger recovery_trigger_; 28 29 ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_; 30 geometry_msgs::PoseStamped oscillation_pose_; 31 pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_; 32 pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_; 33 pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_; 34 35 //触发哪种规划器 36 std::vector<geometry_msgs::PoseStamped>* planner_plan_;//保存最新规划的路径,传给latest_plan_ 37 std::vector<geometry_msgs::PoseStamped>* latest_plan_;//在executeCycle中传给controller_plan_ 38 std::vector<geometry_msgs::PoseStamped>* controller_plan_; 39 40 //规划器线程 41 bool runPlanner_; 42 boost::recursive_mutex planner_mutex_; 43 boost::condition_variable_any planner_cond_; 44 geometry_msgs::PoseStamped planner_goal_; 45 boost::thread* planner_thread_; 46 47 48 boost::recursive_mutex configuration_mutex_; 49 dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_; 50 51 void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level); 52 53 move_base::MoveBaseConfig last_config_; 54 move_base::MoveBaseConfig default_config_; 55 bool setup_, p_freq_change_, c_freq_change_; 56 bool new_global_plan_;
四、正文来了
文件move_base.cpp定义了上面头文件里写的函数,接下来挨个捋一遍:
入口是构造函数MoveBase::MoveBase。
首先,初始化了一堆参数:
1 tf_(tf), //tf2_ros::Buffer& 引用?取址? 2 as_(NULL), //MoveBaseActionServer* 指针 3 planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),//costmap的实例化指针 4 bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"), //nav_core::BaseGlobalPlanner类型的插件 5 blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),//nav_core::BaseLocalPlanner类型的插件 6 recovery_loader_("nav_core", "nav_core::RecoveryBehavior"), //nav_core::RecoveryBehavior类型的插件 7 planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),//三种规划器,看触发哪种 8 runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), //配置参数 9 new_global_plan_(false) //配置参数
1 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
这个时候调用回调函数MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal),具体如下:
如果目标非法,则直接返回:
1 if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){ 2 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); 3 return; 4 }
其中,isQuaternionValid(const geometry_msgs::Quaternion& q)函数如下,主要用于检查四元数的合法性:
1 bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){ 2 //1、首先检查四元数是否元素完整 3 if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){ 4 ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal"); 5 return false; 6 } 7 tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); 8 //2、检查四元数是否趋近于0 9 if(tf_q.length2() < 1e-6){ 10 ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal"); 11 return false; 12 } 13 //3、对四元数规范化,转化为vector 14 tf_q.normalize(); 15 tf2::Vector3 up(0, 0, 1); 16 double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle())); 17 if(fabs(dot - 1) > 1e-3){ 18 ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical."); 19 return false; 20 } 21 22 return true; 23 }
ok,回到回调函数继续看——
1 geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
其中,函数MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg)如下:
1 geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){ 2 std::string global_frame = planner_costmap_ros_->getGlobalFrameID(); 3 geometry_msgs::PoseStamped goal_pose, global_pose; 4 goal_pose = goal_pose_msg; 5 6 //tf一下 7 goal_pose.header.stamp = ros::Time(); 8 9 try{ 10 tf_.transform(goal_pose_msg, global_pose, global_frame); 11 } 12 catch(tf2::TransformException& ex){ 13 ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s", 14 goal_pose.header.frame_id.c_str(), global_frame.c_str(), ex.what()); 15 return goal_pose_msg; 16 } 17 18 return global_pose; 19 }
ok,回到回调函数继续看——
设置目标点并唤醒路径规划线程:
1 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 2 planner_goal_ = goal; 3 runPlanner_ = true; 4 planner_cond_.notify_one(); 5 lock.unlock();
然后发布goal,设置控制频率:
1 current_goal_pub_.publish(goal); 2 std::vector<geometry_msgs::PoseStamped> global_plan; 3 ros::Rate r(controller_frequency_);
1 if(shutdown_costmaps_){ 2 ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously"); 3 planner_costmap_ros_->start(); 4 controller_costmap_ros_->start(); 5 }
重置时间标志位:
1 last_valid_control_ = ros::Time::now(); 2 last_valid_plan_ = ros::Time::now(); 3 last_oscillation_reset_ = ros::Time::now(); 4 planning_retries_ = 0;
1 while(n.ok()) 2 { 3 4 //7. 修改循环频率 5 if(c_freq_change_) 6 { 7 ROS_INFO("Setting controller frequency to %.2f", controller_frequency_); 8 r = ros::Rate(controller_frequency_); 9 c_freq_change_ = false; 10 } 11 12 //8. 如果获得一个抢占式目标 13 if(as_->isPreemptRequested()){ //action的抢断函数 14 if(as_->isNewGoalAvailable()){ 15 //如果有新的目标,会接受的,但不会关闭其他进程 16 move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal(); 17 18 //9.如果目标无效,则返回 19 if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){ 20 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); 21 return; 22 } 23 //10.将目标转换为全局坐标系 24 goal = goalToGlobalFrame(new_goal.target_pose); 25 26 //we‘ll make sure that we reset our state for the next execution cycle 27 //11.设置状态为PLANNING 28 recovery_index_ = 0; 29 state_ = PLANNING; 30 31 //we have a new goal so make sure the planner is awake 32 //12. 设置目标点并唤醒路径规划线程 33 lock.lock(); 34 planner_goal_ = goal; 35 runPlanner_ = true; 36 planner_cond_.notify_one(); 37 lock.unlock(); 38 39 //13. 把goal发布给可视化工具 40 ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y); 41 current_goal_pub_.publish(goal); 42 43 //make sure to reset our timeouts and counters 44 //14. 重置规划时间 45 last_valid_control_ = ros::Time::now(); 46 last_valid_plan_ = ros::Time::now(); 47 last_oscillation_reset_ = ros::Time::now(); 48 planning_retries_ = 0; 49 } 50 else { 51 52 //14.重置状态,设置为抢占式任务 53 //if we‘ve been preempted explicitly we need to shut things down 54 resetState(); 55 56 //通知ActionServer已成功抢占 57 ROS_DEBUG_NAMED("move_base","Move base preempting the current goal"); 58 as_->setPreempted(); 59 60 //we‘ll actually return from execute after preempting 61 return; 62 } 63 } 64 65 //we also want to check if we‘ve changed global frames because we need to transform our goal pose 66 //15.如果目标点的坐标系和全局地图的坐标系不相同 67 if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){ 68 //16,转换目标点坐标系 69 goal = goalToGlobalFrame(goal); 70 71 //we want to go back to the planning state for the next execution cycle 72 recovery_index_ = 0; 73 state_ = PLANNING; 74 75 //17. 设置目标点并唤醒路径规划线程 76 lock.lock(); 77 planner_goal_ = goal; 78 runPlanner_ = true; 79 planner_cond_.notify_one(); 80 lock.unlock(); 81 82 //publish the goal point to the visualizer 83 ROS_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); 84 current_goal_pub_.publish(goal); 85 86 //18.重置规划器相关时间标志位 87 last_valid_control_ = ros::Time::now(); 88 last_valid_plan_ = ros::Time::now(); 89 last_oscillation_reset_ = ros::Time::now(); 90 planning_retries_ = 0; 91 } 92 93 //for timing that gives real time even in simulation 94 ros::WallTime start = ros::WallTime::now(); 95 96 //19. 到达目标点的真正工作,控制机器人进行跟随 97 bool done = executeCycle(goal, global_plan); 98 99 //20.如果完成任务则返回 100 if(done) 101 return; 102 103 //check if execution of the goal has completed in some way 104 105 ros::WallDuration t_diff = ros::WallTime::now() - start; 106 ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec()); 107 //21. 执行休眠动作 108 r.sleep(); 109 //make sure to sleep for the remainder of our cycle time 110 if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING) 111 ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec()); 112 }
其中,done的值即为MoveBase::executeCycle()函数的返回值,这个值非常重要,直接判断了是否到达目标点;MoveBase::executeCycle()函数是控制机器人进行跟随的函数(重要!!!),如下:
获取机器人当前位置:
1 geometry_msgs::PoseStamped global_pose; 2 getRobotPose(global_pose, planner_costmap_ros_); 3 const geometry_msgs::PoseStamped& current_position = global_pose; 4 5 //push the feedback out 6 move_base_msgs::MoveBaseFeedback feedback; 7 feedback.base_position = current_position; 8 as_->publishFeedback(feedback);
其中,getRobotPose()函数如下,需要对准坐标系和时间戳:
1 bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap) 2 { 3 tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose); 4 geometry_msgs::PoseStamped robot_pose; 5 tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose); 6 robot_pose.header.frame_id = robot_base_frame_; 7 robot_pose.header.stamp = ros::Time(); // latest available 8 ros::Time current_time = ros::Time::now(); // save time for checking tf delay later 9 10 // 转换到统一的全局坐标 11 try 12 { 13 tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID()); 14 } 15 catch (tf2::LookupException& ex) 16 { 17 ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what()); 18 return false; 19 } 20 catch (tf2::ConnectivityException& ex) 21 { 22 ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what()); 23 return false; 24 } 25 catch (tf2::ExtrapolationException& ex) 26 { 27 ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what()); 28 return false; 29 } 30 31 // 全局坐标时间戳是否在costmap要求下 32 if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance()) 33 { 34 ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " 35 "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(), 36 current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance()); 37 return false; 38 } 39 40 return true; 41 }
ok,返回MoveBase::executeCycle()函数继续看——
判断当前位置和是否振荡,其中distance函数返回的是两个位置的直线距离(欧氏距离),recovery_trigger_是枚举RecoveryTrigger的对象:
1 if(distance(current_position, oscillation_pose_) >= oscillation_distance_) 2 { 3 last_oscillation_reset_ = ros::Time::now(); 4 oscillation_pose_ = current_position; 5 6 //如果上次的恢复是由振荡引起的,重置恢复指数 7 if(recovery_trigger_ == OSCILLATION_R) 8 recovery_index_ = 0; 9 }
1 if(!controller_costmap_ros_->isCurrent()){ 2 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()); 3 publishZeroVelocity(); 4 return false; 5 }
1 if(new_global_plan_){ 2 //make sure to set the new plan flag to false 3 new_global_plan_ = false; 4 5 ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers"); 6 7 //do a pointer swap under mutex 8 std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_; 9 10 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 11 controller_plan_ = latest_plan_; 12 latest_plan_ = temp_plan; 13 lock.unlock(); 14 ROS_DEBUG_NAMED("move_base","pointers swapped!"); 15 16 //5. 给控制器设置全局路径 17 if(!tc_->setPlan(*controller_plan_)){ 18 //ABORT and SHUTDOWN COSTMAPS 19 ROS_ERROR("Failed to pass global plan to the controller, aborting."); 20 resetState(); 21 22 //disable the planner thread 23 lock.lock(); 24 runPlanner_ = false; 25 lock.unlock(); 26 //6.设置动作中断,返回true 27 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller."); 28 return true; 29 } 30 31 //如果全局路径有效,则不需要recovery 32 if(recovery_trigger_ == PLANNING_R) 33 recovery_index_ = 0; 34 }
其中,tc_是局部规划器的指针,setPlan是TrajectoryPlannerROS的函数(注意!!!跟外部包有关系了!!)
然后判断move_base状态,一般默认状态或者接收到一个有效goal时是PLANNING,在规划出全局路径后state_会由PLANNING变为CONTROLLING,如果规划失败则由PLANNING变为CLEARING,架构如下:
1 switch(state_){ 2 case PLANNING: 3 case CONTROLLING: 4 case CLEARING: 5 default: 6 }
其中,详细介绍一下各个状态:
1 case PLANNING: 2 { 3 boost::recursive_mutex::scoped_lock lock(planner_mutex_); 4 runPlanner_ = true; 5 planner_cond_.notify_one();//还在PLANNING中则唤醒规划线程让它干活 6 } 7 ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state."); 8 break;
(2)机器人控制状态,尝试获取一个有效的速度命令:
1 case CONTROLLING:
如果到达目标点,重置状态,设置动作成功,返回true,其中isGoalReached()函数是TrajectoryPlannerROS的函数(注意!!!这里跟外部包有关!!):
1 if(tc_->isGoalReached()){ 2 ROS_DEBUG_NAMED("move_base","Goal reached!"); 3 resetState(); 4 5 //disable the planner thread 6 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 7 runPlanner_ = false; 8 lock.unlock(); 9 10 as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached."); 11 return true; 12 }
其中,resetState()函数如下,配合上面的看,机器人到达目标点后把move_base状态设置为PLANNING:
1 void MoveBase::resetState(){ 2 // Disable the planner thread 3 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 4 runPlanner_ = false; 5 lock.unlock(); 6 7 // Reset statemachine 8 state_ = PLANNING; 9 recovery_index_ = 0; 10 recovery_trigger_ = PLANNING_R; 11 publishZeroVelocity(); 12 13 //if we shutdown our costmaps when we‘re deactivated... we‘ll do that now 14 if(shutdown_costmaps_){ 15 ROS_DEBUG_NAMED("move_base","Stopping costmaps"); 16 planner_costmap_ros_->stop(); 17 controller_costmap_ros_->stop(); 18 } 19 }
返回去继续看,如果超过震荡时间,停止机器人,设置清障标志位:
1 if(oscillation_timeout_ > 0.0 && 2 last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now()) 3 { 4 publishZeroVelocity(); 5 state_ = CLEARING; 6 recovery_trigger_ = OSCILLATION_R; 7 }
1 if(tc_->computeVelocityCommands(cmd_vel)){ 2 ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf", 3 cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z ); 4 last_valid_control_ = ros::Time::now(); 5 //make sure that we send the velocity command to the base 6 vel_pub_.publish(cmd_vel); 7 if(recovery_trigger_ == CONTROLLING_R) 8 recovery_index_ = 0; 9 }
其中,computeVelocityCommands函数又是TrajectoryPlannerROS的函数(注意!!!这里跟外部包有关!!)。
如果没有获取有效速度,则判断是否超过尝试时间,如果超时,则停止机器人,进入清障模式:
1 if(ros::Time::now() > attempt_end){ 2 //we‘ll move into our obstacle clearing mode 3 publishZeroVelocity(); 4 state_ = CLEARING; 5 recovery_trigger_ = CONTROLLING_R; 6 }
如果没有超时,则再全局规划一个新的路径:
1 last_valid_plan_ = ros::Time::now(); 2 planning_retries_ = 0; 3 state_ = PLANNING; 4 publishZeroVelocity(); 5 6 //enable the planner thread in case it isn‘t running on a clock 7 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 8 runPlanner_ = true; 9 planner_cond_.notify_one(); 10 lock.unlock();
(3)机器人清障状态,规划或者控制失败,恢复或者进入到清障模式:
1 case CLEARING:
如果有可用恢复器,执行恢复动作,并设置状态为PLANNING:
1 if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){ 2 ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size()); 3 recovery_behaviors_[recovery_index_]->runBehavior(); 4 5 //we at least want to give the robot some time to stop oscillating after executing the behavior 6 last_oscillation_reset_ = ros::Time::now(); 7 8 //we‘ll check if the recovery behavior actually worked 9 ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state"); 10 last_valid_plan_ = ros::Time::now(); 11 planning_retries_ = 0; 12 state_ = PLANNING; 13 14 //update the index of the next recovery behavior that we‘ll try 15 recovery_index_++; 16 }
其中,recovery_behaviors_类型为std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> >,runBehavior()函数为move_slow_and_clear包里面的函数。(注意!!!链接到外部包!!!)。
如果没有可用恢复器,结束动作,返回true:
1 ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it."); 2 //disable the planner thread 3 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 4 runPlanner_ = false; 5 lock.unlock(); 6 7 ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this."); 8 9 if(recovery_trigger_ == CONTROLLING_R){ 10 ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors"); 11 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors."); 12 } 13 else if(recovery_trigger_ == PLANNING_R){ 14 ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors"); 15 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors."); 16 } 17 else if(recovery_trigger_ == OSCILLATION_R){ 18 ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors"); 19 as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors."); 20 } 21 resetState(); 22 return true;
(4)除了上述状态以外:
1 default: 2 ROS_ERROR("This case should never be reached, something is wrong, aborting"); 3 resetState(); 4 //disable the planner thread 5 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 6 runPlanner_ = false; 7 lock.unlock(); 8 as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it."); 9 return true;
ok,MoveBase::executeCycle()函数终于介绍完了,回到回调函数继续看——
剩下的就是解放线程,反馈给action结果:
1 //22. 唤醒计划线程,以便它可以干净地退出 2 lock.lock(); 3 runPlanner_ = true; 4 planner_cond_.notify_one(); 5 lock.unlock(); 6 7 //23. 如果节点结束就终止并返回 8 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed"); 9 return;
ok,MoveBase::executeCb()函数看完了,回到构造函数MoveBase::MoveBase——
触发模式(三种模式:规划、控制、振荡)设置为“规划中”:
1 recovery_trigger_ = PLANNING_R;
1 std::string global_planner, local_planner; 2 private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS")); 3 private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS")); 4 private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link")); 5 private_nh.param("global_costmap/global_frame", global_frame_, std::string("map")); 6 private_nh.param("planner_frequency", planner_frequency_, 0.0); 7 private_nh.param("controller_frequency", controller_frequency_, 20.0); 8 private_nh.param("planner_patience", planner_patience_, 5.0); 9 private_nh.param("controller_patience", controller_patience_, 15.0); 10 private_nh.param("max_planning_retries", max_planning_retries_, -1); // disabled by default 11 private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0); 12 private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
为三种规划器(planner_plan_保存最新规划的路径,传递给latest_plan_,然后latest_plan_通过executeCycle中传给controller_plan_)设置内存缓冲区:
1 planner_plan_ = new std::vector<geometry_msgs::PoseStamped>(); 2 latest_plan_ = new std::vector<geometry_msgs::PoseStamped>(); 3 controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
设置规划器线程,planner_thread_是boost::thread*类型的指针:
1 planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
其中,MoveBase::planThread()函数是planner线程的入口。这个函数需要等待actionlib服务器的cbMoveBase::executeCb来唤醒启动。主要作用是调用全局路径规划获取路径,同时保证规划的周期性以及规划超时清除goal,如下:
1 void MoveBase::planThread(){ 2 ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread..."); 3 ros::NodeHandle n; 4 ros::Timer timer; 5 bool wait_for_wake = false; 6 //1. 创建递归锁 7 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 8 while(n.ok()){ 9 //check if we should run the planner (the mutex is locked) 10 //2.判断是否阻塞线程 11 while(wait_for_wake || !runPlanner_){ 12 //if we should not be running the planner then suspend this thread 13 ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending"); 14 //当 std::condition_variable 对象的某个 wait 函数被调用的时候, 15 //它使用 std::unique_lock(通过 std::mutex) 来锁住当前线程。 16 //当前线程会一直被阻塞,直到另外一个线程在相同的 std::condition_variable 对象上调用了 notification 函数来唤醒当前线程。 17 planner_cond_.wait(lock); 18 wait_for_wake = false; 19 } 20 ros::Time start_time = ros::Time::now(); 21 22 //time to plan! get a copy of the goal and unlock the mutex 23 geometry_msgs::PoseStamped temp_goal = planner_goal_; 24 lock.unlock(); 25 ROS_DEBUG_NAMED("move_base_plan_thread","Planning..."); 26 27 //run planner 28 29 //3. 获取规划的全局路径 30 //这里的makePlan作用是获取机器人的位姿作为起点,然后调用全局规划器的makePlan返回规划路径,存储在planner_plan_ 31 planner_plan_->clear(); 32 bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_); 33 34 35 //4.如果获得了plan,则将其赋值给latest_plan_ 36 if(gotPlan){ 37 ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size()); 38 //pointer swap the plans under mutex (the controller will pull from latest_plan_) 39 std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_; 40 41 lock.lock(); 42 planner_plan_ = latest_plan_; 43 latest_plan_ = temp_plan; 44 last_valid_plan_ = ros::Time::now(); 45 planning_retries_ = 0; 46 new_global_plan_ = true; 47 48 ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner"); 49 50 //make sure we only start the controller if we still haven‘t reached the goal 51 if(runPlanner_) 52 state_ = CONTROLLING; 53 if(planner_frequency_ <= 0) 54 runPlanner_ = false; 55 lock.unlock(); 56 } 57 58 //5. 达到一定条件后停止路径规划,进入清障模式 59 //如果没有规划出路径,并且处于PLANNING状态,则判断是否超过最大规划周期或者规划次数 60 //如果是则进入自转模式,否则应该会等待MoveBase::executeCycle的唤醒再次规划 61 else if(state_==PLANNING){ 62 //仅在MoveBase::executeCb及其调用的MoveBase::executeCycle或者重置状态时会被设置为PLANNING 63 //一般是刚获得新目标,或者得到路径但计算不出下一步控制时重新进行路径规划 64 ROS_DEBUG_NAMED("move_base_plan_thread","No Plan..."); 65 ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_); 66 67 //check if we‘ve tried to make a plan for over our time limit or our maximum number of retries 68 //issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_ 69 //is negative (the default), it is just ignored and we have the same behavior as ever 70 lock.lock(); 71 planning_retries_++; 72 if(runPlanner_ && 73 (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){ 74 //we‘ll move into our obstacle clearing mode 75 state_ = CLEARING; 76 runPlanner_ = false; // proper solution for issue #523 77 publishZeroVelocity(); 78 recovery_trigger_ = PLANNING_R; 79 } 80 81 lock.unlock(); 82 } 83 84 //take the mutex for the next iteration 85 lock.lock(); 86 87 88 //6.设置睡眠模式 89 //如果还没到规划周期则定时器睡眠,在定时器中断中通过planner_cond_唤醒,这里规划周期为0 90 if(planner_frequency_ > 0){ 91 ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now(); 92 if (sleep_time > ros::Duration(0.0)){ 93 wait_for_wake = true; 94 timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this); 95 } 96 } 97 } 98 }
其中,planner_cond_专门用于开启路径规划的线程,在其他地方也经常遇到;这一步中,实现了从latest_plan_到planner_plan_ 的跨越;MoveBase::wakePlanner()函数中用planner_cond_开启了路径规划的线程。
ok,MoveBase::planThread()看完了,回到构造函数MoveBase::MoveBase继续看——
创建发布者,话题名一个是cmd_vel,一个是cunrrent_goal,一个是goal:
1 vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1); 2 current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 ); 3 4 ros::NodeHandle action_nh("move_base"); 5 action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
1 ros::NodeHandle simple_nh("move_base_simple"); 2 goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
其中,我们来看MoveBase::goalCB()函数,传入的是goal,主要作用是为rviz等提供调用借口,将geometry_msgs::PoseStamped形式的goal转换成move_base_msgs::MoveBaseActionGoal,再发布到对应类型的goal话题中:
1 void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){ 2 ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server."); 3 move_base_msgs::MoveBaseActionGoal action_goal; 4 action_goal.header.stamp = ros::Time::now(); 5 action_goal.goal.target_pose = *goal; 6 7 action_goal_pub_.publish(action_goal); 8 }
ok,MoveBase::goalCB()函数看完了,回到构造函数MoveBase::MoveBase继续看——
设置costmap参数,技巧是把膨胀层设置为大于机器人的半径:
1 private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325); 2 private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46); 3 private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_); 4 private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0); 5 6 private_nh.param("shutdown_costmaps", shutdown_costmaps_, false); 7 private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true); 8 private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);
设置全局路径规划器,planner_costmap_ros_是costmap_2d::Costmap2DROS*类型的实例化指针,planner_是boost::shared_ptr<nav_core::BaseGlobalPlanner>类型:
1 planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_); 2 planner_costmap_ros_->pause(); 3 try { 4 planner_ = bgp_loader_.createInstance(global_planner); 5 planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_); 6 } catch (const pluginlib::PluginlibException& ex) { 7 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()); 8 exit(1); 9 }
1 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_); 2 controller_costmap_ros_->pause(); 3 try { 4 tc_ = blp_loader_.createInstance(local_planner); 5 ROS_INFO("Created local_planner %s", local_planner.c_str()); 6 tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_); 7 } catch (const pluginlib::PluginlibException& ex) { 8 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()); 9 exit(1); 10 }
这个时候,tc_就成了局部规划器的实例对象。
开始更新costmap:
1 planner_costmap_ros_->start(); 2 controller_costmap_ros_->start();
全局规划:
1 make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
其中,MoveBase::planService()函数写了全局规划的策略,以多少距离向外搜索路径:
1 bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
获取起始点,如果没有起始点,那就获取当前的全局位置为起始点:
1 geometry_msgs::PoseStamped start; 2 //如果起始点为空,设置global_pose为起始点 3 if(req.start.header.frame_id.empty()) 4 { 5 geometry_msgs::PoseStamped global_pose; 6 if(!getRobotPose(global_pose, planner_costmap_ros_)){ 7 ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot"); 8 return false; 9 } 10 start = global_pose; 11 } 12 else 13 { 14 start = req.start; 15 }
制定规划策略:
1 std::vector<geometry_msgs::PoseStamped> global_plan; 2 if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){ 3 ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance", 4 req.goal.pose.position.x, req.goal.pose.position.y); 5 6 //在规定的公差范围内向外寻找可行的goal 7 geometry_msgs::PoseStamped p; 8 p = req.goal; 9 bool found_legal = false; 10 float resolution = planner_costmap_ros_->getCostmap()->getResolution(); 11 float search_increment = resolution*3.0;//以3倍分辨率的增量向外寻找 12 if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance; 13 for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) { 14 for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) { 15 for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) { 16 17 //不在本位置的外侧layer查找,太近的不找 18 if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue; 19 20 //从两个方向x、y查找精确的goal 21 for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) { 22 23 //第一次遍历如果偏移量过小,则去除这个点或者上一点 24 if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue; 25 26 for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) { 27 if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue; 28 29 p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult; 30 p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult; 31 32 if(planner_->makePlan(start, p, global_plan)){ 33 if(!global_plan.empty()){ 34 35 //adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there 36 //(the reachable goal should have been added by the global planner) 37 global_plan.push_back(req.goal); 38 39 found_legal = true; 40 ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y); 41 break; 42 } 43 } 44 else{ 45 ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y); 46 } 47 } 48 } 49 } 50 } 51 } 52 }
然后把规划后的global_plan附给resp,并且传出去:
1 resp.plan.poses.resize(global_plan.size()); 2 for(unsigned int i = 0; i < global_plan.size(); ++i){ 3 resp.plan.poses[i] = global_plan[i]; 4 }
ok,MoveBase::planService()函数看完了,回到构造函数MoveBase::MoveBase继续看——
开始清除地图服务:
1 clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
其中,调用了MoveBase::clearCostmapsService()函数,提供清除一次costmap的功能:
1 bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){ 2 //clear the costmaps 3 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex())); 4 controller_costmap_ros_->resetLayers(); 5 6 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner(*(planner_costmap_ros_->getCostmap()->getMutex())); 7 planner_costmap_ros_->resetLayers(); 8 return true; 9 }
值得注意的是,其中有一个函数resetLayers(),调用的是costmap包(注意!!外部链接!!!),该函数的功能是重置地图,内部包括重置总地图、重置地图各层:
1 void Costmap2DROS::resetLayers() 2 { 3 Costmap2D* top = layered_costmap_->getCostmap(); 4 top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY()); 5 std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins(); 6 for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end(); 7 ++plugin) 8 { 9 (*plugin)->reset(); 10 } 11 }
在该函数中,首先将总的地图信息重置为缺省值。然后调用各层的reset函数对各层进行重置(第9行)。针对不同的reset函数,功能如下:
1 void StaticLayer::reset() 2 { 3 if (first_map_only_) 4 { 5 has_updated_data_ = true; 6 } 7 else 8 { 9 onInitialize(); 10 } 11 }
1 void ObstacleLayer::reset() 2 { 3 deactivate(); 4 resetMaps(); 5 current_ = true; 6 activate(); 7 }
ok,MoveBase::clearCostmapsService()函数看完了,回到构造函数MoveBase::MoveBase继续看——
如果不小心关闭了costmap, 则停用:
1 if(shutdown_costmaps_){ 2 ROS_DEBUG_NAMED("move_base","Stopping costmaps initially"); 3 planner_costmap_ros_->stop(); 4 controller_costmap_ros_->stop(); 5 }
加载指定的恢复器,加载不出来则使用默认的,这里包括了找不到路自转360:
1 if(!loadRecoveryBehaviors(private_nh)){ 2 loadDefaultRecoveryBehaviors(); 3 }
导航过程基本结束,把状态初始化:
1 //initially, we‘ll need to make a plan 2 state_ = PLANNING; 3 //we‘ll start executing recovery behaviors at the beginning of our list 4 recovery_index_ = 0; 5 //10.开启move_base动作器 6 as_->start();
启动动态参数服务器:
1 dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~")); 2 dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2); 3 dsrv_->setCallback(cb);
其中,回调函数MoveBase::reconfigureCB(),配置了各种参数,感兴趣的可以看下,代码如下:
1 void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){ 2 boost::recursive_mutex::scoped_lock l(configuration_mutex_); 3 4 //一旦被调用,我们要确保有原始配置 5 if(!setup_) 6 { 7 last_config_ = config; 8 default_config_ = config; 9 setup_ = true; 10 return; 11 } 12 13 if(config.restore_defaults) { 14 config = default_config_; 15 //如果有人在参数服务器上设置默认值,要防止循环 16 config.restore_defaults = false; 17 } 18 19 if(planner_frequency_ != config.planner_frequency) 20 { 21 planner_frequency_ = config.planner_frequency; 22 p_freq_change_ = true; 23 } 24 25 if(controller_frequency_ != config.controller_frequency) 26 { 27 controller_frequency_ = config.controller_frequency; 28 c_freq_change_ = true; 29 } 30 31 planner_patience_ = config.planner_patience; 32 controller_patience_ = config.controller_patience; 33 max_planning_retries_ = config.max_planning_retries; 34 conservative_reset_dist_ = config.conservative_reset_dist; 35 36 recovery_behavior_enabled_ = config.recovery_behavior_enabled; 37 clearing_rotation_allowed_ = config.clearing_rotation_allowed; 38 shutdown_costmaps_ = config.shutdown_costmaps; 39 40 oscillation_timeout_ = config.oscillation_timeout; 41 oscillation_distance_ = config.oscillation_distance; 42 if(config.base_global_planner != last_config_.base_global_planner) { 43 boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_; 44 //创建全局规划 45 ROS_INFO("Loading global planner %s", config.base_global_planner.c_str()); 46 try { 47 planner_ = bgp_loader_.createInstance(config.base_global_planner); 48 49 // 等待当前规划结束 50 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 51 52 // 在新规划开始前clear旧的 53 planner_plan_->clear(); 54 latest_plan_->clear(); 55 controller_plan_->clear(); 56 resetState(); 57 planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_); 58 59 lock.unlock(); 60 } catch (const pluginlib::PluginlibException& ex) { 61 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \ 62 containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what()); 63 planner_ = old_planner; 64 config.base_global_planner = last_config_.base_global_planner; 65 } 66 } 67 68 if(config.base_local_planner != last_config_.base_local_planner){ 69 boost::shared_ptr<nav_core::BaseLocalPlanner> old_planner = tc_; 70 //创建局部规划 71 try { 72 tc_ = blp_loader_.createInstance(config.base_local_planner); 73 // 清理旧的 74 planner_plan_->clear(); 75 latest_plan_->clear(); 76 controller_plan_->clear(); 77 resetState(); 78 tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_); 79 } catch (const pluginlib::PluginlibException& ex) { 80 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \ 81 containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what()); 82 tc_ = old_planner; 83 config.base_local_planner = last_config_.base_local_planner; 84 } 85 } 86 87 last_config_ = config; 88 }
ok,构造函数MoveBase::MoveBase看完了,接着到析构函数,释放了内存——
1 MoveBase::~MoveBase(){ 2 recovery_behaviors_.clear(); 3 4 delete dsrv_; 5 6 if(as_ != NULL) 7 delete as_; 8 9 if(planner_costmap_ros_ != NULL) 10 delete planner_costmap_ros_; 11 12 if(controller_costmap_ros_ != NULL) 13 delete controller_costmap_ros_; 14 15 planner_thread_->interrupt(); 16 planner_thread_->join(); 17 18 delete planner_thread_; 19 20 delete planner_plan_; 21 delete latest_plan_; 22 delete controller_plan_; 23 24 planner_.reset(); 25 tc_.reset(); 26 }
至此,move_base全部解析完毕,恭喜你有耐心看完,如果有错误的地方欢迎指出,在此感谢!
五、需要注意的地方
1、move_base与costmap_2d、全局规划器、局部规划器、恢复器紧密相连,很多地方都是调用了这几个包的函数。
2、需要着重关注 头文件 中写的那些枚举常量,以及各个标志位,很多都是正文中的判断条件。
3、多线程、智能指针有必要深入学一下,对内存和任务管理很有帮助。
Navigation(五) Move_base最最全解析(按执行逻辑梳理)
标签:accept 并且 更新 string opp wait std oop on()
原文地址:https://www.cnblogs.com/JuiceCat/p/13040552.html