Navigation（一） move_base源码最全解析
一、概述
目测是全网最全的解析，花了几个小时通读并整理的，供大家参考学习。
本篇是直接源码配注释的，所以逻辑性不够强，我还写了一篇按照代码执行逻辑读代码的文章，个人认为比这篇有用得多，以下为链接，可以配合着看：https://www.cnblogs.com/JuiceCat/p/13040552.html 。
概况的话可以看下古月居 https://www.guyuehome.com/270，其实它是翻译官方的，英语ok的可以去ros wiki翻看原版。
重点：navfn包 全局规划（Dji算法）、base_local_planner包 局部规划（Trajectory Rollout 和Dynamic Window approaches算法）
二、move_base.h
1 #ifndef NAV_MOVE_BASE_ACTION_H_ 2 #define NAV_MOVE_BASE_ACTION_H_ 3 4 #include <vector> 5 #include <string> 6 7 #include <ros/ros.h> 8 9 #include <actionlib/server/simple_action_server.h> 10 #include <move_base_msgs/MoveBaseAction.h> 11 12 #include <nav_core/base_local_planner.h> 13 #include <nav_core/base_global_planner.h> 14 #include <nav_core/recovery_behavior.h> 15 #include <geometry_msgs/PoseStamped.h> 16 #include <costmap_2d/costmap_2d_ros.h> 17 #include <costmap_2d/costmap_2d.h> 18 #include <nav_msgs/GetPlan.h> 19 20 #include <pluginlib/class_loader.hpp> 21 #include <std_srvs/Empty.h> 22 23 #include <dynamic_reconfigure/server.h> 24 #include "move_base/MoveBaseConfig.h" 25 26 namespace move_base { 27 //声明server端，消息类型是move_base_msgs::MoveBaseAction 28 typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer; 29 //movebase状态表示 30 enum MoveBaseState { 31 PLANNING, 32 CONTROLLING, 33 CLEARING 34 }; 35 //触发恢复模式 36 enum RecoveryTrigger 37 { 38 PLANNING_R, 39 CONTROLLING_R, 40 OSCILLATION_R 41 }; 42 43 //使用actionlib：：ActionServer接口的类，该接口将robot移动到目标位置。 44 class MoveBase { 45 public: 46 // 构造函数，传入的参数是tf 47 MoveBase(tf2_ros::Buffer& tf); 48 49 //析构函数 50 virtual ~MoveBase(); 51 52 //控制闭环、全局规划、 到达目标返回true，没有到达返回false 53 bool executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan); 54 55 private: 56 /** 57 * @brief 清除costmap的server端 58 * @param req 表示server的request 59 * @param resp 表示server的response 60 * @return 如果server端被成功调用则为True，否则false 61 */ 62 bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp); 63 64 /** 65 * @brief 当action不活跃时，调用此函数，返回plan 66 * @param req 表示goal的request 67 * @param resp 表示plan的request 68 * @return 规划成功返回reue，否则返回false 69 */ 70 bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp); 71 72 /** 73 * @brief 新的全局规划 74 * @param goal 规划的目标点 75 * @param plan 规划 76 * @return 规划成功则返回True 否则false 77 */ 78 bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan); 79 80 /** 81 * @brief 从参数服务器加载导航参数Load the recovery behaviors 82 * @param node 表示 ros::NodeHandle 加载的参数 83 * @return 加载成功返回True 否则 false 84 */ 85 bool loadRecoveryBehaviors(ros::NodeHandle node); 86 87 // 加载默认导航参数 88 void loadDefaultRecoveryBehaviors(); 89 90 /** 91 * @brief 清楚机器人局部规划框的障碍物 92 * @param size_x 局部规划框的长x 93 * @param size_y 局部规划框的宽y 94 */ 95 void clearCostmapWindows(double size_x, double size_y); 96 97 //发布速度为0的指令 98 void publishZeroVelocity(); 99 100 // 重置move_base action的状态，设置速度为0 101 void resetState(); 102 103 void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal); 104 105 void planThread(); 106 107 void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal); 108 109 bool isQuaternionValid(const geometry_msgs::Quaternion& q); 110 111 bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap); 112 113 double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2); 114 115 geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg); 116 117 //周期性地唤醒规划器 118 void wakePlanner(const ros::TimerEvent& event); 119 120 tf2_ros::Buffer& tf_; 121 122 MoveBaseActionServer* as_; //就是actionlib的server端 123 124 boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;//局部规划器，加载并创建实例后的指针 125 costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;//costmap的实例化指针 126 127 boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;//全局规划器，加载并创建实例后的指针 128 std::string robot_base_frame_, global_frame_; 129 130 std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;//可能是出错后的恢复 131 unsigned int recovery_index_; 132 133 geometry_msgs::PoseStamped global_pose_; 134 double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_; 135 double planner_patience_, controller_patience_; 136 int32_t max_planning_retries_; 137 uint32_t planning_retries_; 138 double conservative_reset_dist_, clearing_radius_; 139 ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_; 140 ros::Subscriber goal_sub_; 141 ros::ServiceServer make_plan_srv_, clear_costmaps_srv_; 142 bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_; 143 double oscillation_timeout_, oscillation_distance_; 144 145 MoveBaseState state_; 146 RecoveryTrigger recovery_trigger_; 147 148 ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_; 149 geometry_msgs::PoseStamped oscillation_pose_; 150 pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_; 151 pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_; 152 pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_; 153 154 //触发哪种规划器 155 std::vector<geometry_msgs::PoseStamped>* planner_plan_;//保存最新规划的路径，传给latest_plan_ 156 std::vector<geometry_msgs::PoseStamped>* latest_plan_;//在executeCycle中传给controller_plan_ 157 std::vector<geometry_msgs::PoseStamped>* controller_plan_; 158 159 //规划器线程 160 bool runPlanner_; 161 boost::recursive_mutex planner_mutex_; 162 boost::condition_variable_any planner_cond_; 163 geometry_msgs::PoseStamped planner_goal_; 164 boost::thread* planner_thread_; 165 166 167 boost::recursive_mutex configuration_mutex_; 168 dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_; 169 170 void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level); 171 172 move_base::MoveBaseConfig last_config_; 173 move_base::MoveBaseConfig default_config_; 174 bool setup_, p_freq_change_, c_freq_change_; 175 bool new_global_plan_; 176 }; 177 }; 178 #endif
三、move_base_node.cpp
1 #include <move_base/move_base.h> 2 #include <tf2_ros/transform_listener.h> 3 4 int main(int argc, char** argv){ 5 ros::init(argc, argv, "move_base_node"); 6 tf2_ros::Buffer buffer(ros::Duration(10)); 7 tf2_ros::TransformListener tf(buffer); 8 9 move_base::MoveBase move_base( buffer );//本cpp中只调用了这个构造函数 10 11 //ros::MultiThreadedSpinner s; 12 ros::spin(); 13 14 return(0); 15 }
四、move_base.cpp
1 #include <move_base/move_base.h> 2 #include <cmath> 3 4 #include <boost/algorithm/string.hpp> 5 #include <boost/thread.hpp> 6 7 #include <geometry_msgs/Twist.h> 8 9 #include <tf2_geometry_msgs/tf2_geometry_msgs.h> 10 11 namespace move_base { 12 13 MoveBase::MoveBase(tf2_ros::Buffer& tf) : 14 tf_(tf), 15 as_(NULL), 16 planner_costmap_ros_(NULL), controller_costmap_ros_(NULL), 17 bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"), 18 blp_loader_("nav_core", "nav_core::BaseLocalPlanner"), 19 recovery_loader_("nav_core", "nav_core::RecoveryBehavior"), 20 planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), 21 runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) { 22 23 24 //1. 创建move_base action,绑定回调函数。定义一个名为move_base的SimpleActionServer。该服务器的Callback为MoveBase::executeCb。 25 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false); 26 27 ros::NodeHandle private_nh("~"); 28 ros::NodeHandle nh; 29 30 31 recovery_trigger_ = PLANNING_R; 32 33 //2.加载参数。从参数服务器获取一些参数，包括两个规划器名称、代价地图坐标系、规划频率、控制周期等 34 std::string global_planner, local_planner; 35 private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS")); 36 private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS")); 37 private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link")); 38 private_nh.param("global_costmap/global_frame", global_frame_, std::string("map")); 39 private_nh.param("planner_frequency", planner_frequency_, 0.0); 40 private_nh.param("controller_frequency", controller_frequency_, 20.0); 41 private_nh.param("planner_patience", planner_patience_, 5.0); 42 private_nh.param("controller_patience", controller_patience_, 15.0); 43 private_nh.param("max_planning_retries", max_planning_retries_, -1); // disabled by default 44 45 private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0); 46 private_nh.param("oscillation_distance", oscillation_distance_, 0.5); 47 48 49 //set up plan triple buffer 50 planner_plan_ = new std::vector<geometry_msgs::PoseStamped>(); 51 latest_plan_ = new std::vector<geometry_msgs::PoseStamped>(); 52 controller_plan_ = new std::vector<geometry_msgs::PoseStamped>(); 53 54 //3. 设置规划器线程 55 //set up the planner's thread 56 planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this)); 57 58 //4. 创建发布者 59 //for commanding the base 60 vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1); 61 current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 ); 62 63 ros::NodeHandle action_nh("move_base"); 64 action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1); 65 66 //提供消息类型为geometry_msgs::PoseStamped的发送goals的接口，比如cb为MoveBase::goalCB，在rviz中输入的目标点就是通过这个函数来响应的 67 ros::NodeHandle simple_nh("move_base_simple"); 68 goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1)); 69 70 //我们假设机器人的半径与costmap的规定一致 71 private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325); 72 private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46); 73 private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_); 74 private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0); 75 76 private_nh.param("shutdown_costmaps", shutdown_costmaps_, false); 77 private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true); 78 private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true); 79 80 // 5. 设置全局路径规划器 81 //create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map 82 planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_); 83 planner_costmap_ros_->pause(); 84 85 //initialize the global planner 86 try { 87 planner_ = bgp_loader_.createInstance(global_planner); 88 planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_); 89 } catch (const pluginlib::PluginlibException& ex) { 90 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()); 91 exit(1); 92 } 93 94 // 6.设置局部路径规划器 95 //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map 96 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_); 97 controller_costmap_ros_->pause(); 98 99 //create a local planner 100 try { 101 tc_ = blp_loader_.createInstance(local_planner); 102 ROS_INFO("Created local_planner %s", local_planner.c_str()); 103 tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_); 104 } catch (const pluginlib::PluginlibException& ex) { 105 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()); 106 exit(1); 107 } 108 109 // Start actively updating costmaps based on sensor data 110 111 //7.开始更新costmap 112 planner_costmap_ros_->start(); 113 controller_costmap_ros_->start(); 114 115 //advertise a service for getting a plan 116 //8.开启创建地图,清除地图服务 117 make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this); 118 119 //定义一个名为clear_costmaps的服务，cb为MoveBase::clearCostmapsService 120 clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this); 121 122 //如果不小心关闭了costmap 123 if(shutdown_costmaps_){ 124 ROS_DEBUG_NAMED("move_base","Stopping costmaps initially"); 125 planner_costmap_ros_->stop(); 126 controller_costmap_ros_->stop(); 127 } 128 129 //9.加载指定的恢复器 130 if(!loadRecoveryBehaviors(private_nh)){ 131 loadDefaultRecoveryBehaviors();//先loadRecoveryBehaviors，不行再loadDefaultRecoveryBehaviors加载默认的，这里包括了找不到路自转360 132 } 133 134 //initially, we'll need to make a plan 135 state_ = PLANNING; 136 137 //we'll start executing recovery behaviors at the beginning of our list 138 recovery_index_ = 0; 139 140 //10.开启move_base动作器 141 as_->start(); 142 //启动动态参数服务器，回调函数为reconfigureCB，推荐看一下古月居https://www.guyuehome.com/1173 143 dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~")); 144 dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2); 145 dsrv_->setCallback(cb); 146 } 147 148 void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){ 149 boost::recursive_mutex::scoped_lock l(configuration_mutex_); 150 151 //一旦被调用，我们要确保有原始配置 152 if(!setup_) 153 { 154 last_config_ = config; 155 default_config_ = config; 156 setup_ = true; 157 return; 158 } 159 160 if(config.restore_defaults) { 161 config = default_config_; 162 //如果有人在参数服务器上设置默认值，要防止循环 163 config.restore_defaults = false; 164 } 165 166 if(planner_frequency_ != config.planner_frequency) 167 { 168 planner_frequency_ = config.planner_frequency; 169 p_freq_change_ = true; 170 } 171 172 if(controller_frequency_ != config.controller_frequency) 173 { 174 controller_frequency_ = config.controller_frequency; 175 c_freq_change_ = true; 176 } 177 178 planner_patience_ = config.planner_patience; 179 controller_patience_ = config.controller_patience; 180 max_planning_retries_ = config.max_planning_retries; 181 conservative_reset_dist_ = config.conservative_reset_dist; 182 183 recovery_behavior_enabled_ = config.recovery_behavior_enabled; 184 clearing_rotation_allowed_ = config.clearing_rotation_allowed; 185 shutdown_costmaps_ = config.shutdown_costmaps; 186 187 oscillation_timeout_ = config.oscillation_timeout; 188 oscillation_distance_ = config.oscillation_distance; 189 if(config.base_global_planner != last_config_.base_global_planner) { 190 boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_; 191 //创建全局规划 192 ROS_INFO("Loading global planner %s", config.base_global_planner.c_str()); 193 try { 194 planner_ = bgp_loader_.createInstance(config.base_global_planner); 195 196 // 等待当前规划结束 197 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 198 199 // 在新规划开始前clear旧的 200 planner_plan_->clear(); 201 latest_plan_->clear(); 202 controller_plan_->clear(); 203 resetState(); 204 planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_); 205 206 lock.unlock(); 207 } catch (const pluginlib::PluginlibException& ex) { 208 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \ 209 containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what()); 210 planner_ = old_planner; 211 config.base_global_planner = last_config_.base_global_planner; 212 } 213 } 214 215 if(config.base_local_planner != last_config_.base_local_planner){ 216 boost::shared_ptr<nav_core::BaseLocalPlanner> old_planner = tc_; 217 //创建局部规划 218 try { 219 tc_ = blp_loader_.createInstance(config.base_local_planner); 220 // 清理旧的 221 planner_plan_->clear(); 222 latest_plan_->clear(); 223 controller_plan_->clear(); 224 resetState(); 225 tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_); 226 } catch (const pluginlib::PluginlibException& ex) { 227 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \ 228 containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what()); 229 tc_ = old_planner; 230 config.base_local_planner = last_config_.base_local_planner; 231 } 232 } 233 234 last_config_ = config; 235 } 236 237 //为rviz等提供调用借口，将geometry_msgs::PoseStamped形式的goal转换成move_base_msgs::MoveBaseActionGoal，再发布到对应类型的goal话题中 238 void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){ 239 ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server."); 240 move_base_msgs::MoveBaseActionGoal action_goal; 241 action_goal.header.stamp = ros::Time::now(); 242 action_goal.goal.target_pose = *goal; 243 244 action_goal_pub_.publish(action_goal); 245 } 246 247 void MoveBase::clearCostmapWindows(double size_x, double size_y){ 248 geometry_msgs::PoseStamped global_pose; 249 250 //clear the planner's costmap 251 getRobotPose(global_pose, planner_costmap_ros_); 252 253 std::vector<geometry_msgs::Point> clear_poly; 254 double x = global_pose.pose.position.x; 255 double y = global_pose.pose.position.y; 256 geometry_msgs::Point pt; 257 258 pt.x = x - size_x / 2; 259 pt.y = y - size_y / 2; 260 clear_poly.push_back(pt); 261 262 pt.x = x + size_x / 2; 263 pt.y = y - size_y / 2; 264 clear_poly.push_back(pt); 265 266 pt.x = x + size_x / 2; 267 pt.y = y + size_y / 2; 268 clear_poly.push_back(pt); 269 270 pt.x = x - size_x / 2; 271 pt.y = y + size_y / 2; 272 clear_poly.push_back(pt); 273 274 planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE); 275 276 //clear the controller's costmap 277 getRobotPose(global_pose, controller_costmap_ros_); 278 279 clear_poly.clear(); 280 x = global_pose.pose.position.x; 281 y = global_pose.pose.position.y; 282 283 pt.x = x - size_x / 2; 284 pt.y = y - size_y / 2; 285 clear_poly.push_back(pt); 286 287 pt.x = x + size_x / 2; 288 pt.y = y - size_y / 2; 289 clear_poly.push_back(pt); 290 291 pt.x = x + size_x / 2; 292 pt.y = y + size_y / 2; 293 clear_poly.push_back(pt); 294 295 pt.x = x - size_x / 2; 296 pt.y = y + size_y / 2; 297 clear_poly.push_back(pt); 298 299 controller_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE); 300 } 301 302 bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){ 303 //clear the costmaps 304 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex())); 305 controller_costmap_ros_->resetLayers(); 306 307 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner(*(planner_costmap_ros_->getCostmap()->getMutex())); 308 planner_costmap_ros_->resetLayers(); 309 return true; 310 } 311 312 313 bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){ 314 if(as_->isActive()){ 315 ROS_ERROR("move_base must be in an inactive state to make a plan for an external user"); 316 return false; 317 } 318 //make sure we have a costmap for our planner 319 if(planner_costmap_ros_ == NULL){ 320 ROS_ERROR("move_base cannot make a plan for you because it doesn't have a costmap"); 321 return false; 322 } 323 324 //1. 获取起始点 325 geometry_msgs::PoseStamped start; 326 //如果起始点为空，设置global_pose为起始点 327 if(req.start.header.frame_id.empty()) 328 { 329 geometry_msgs::PoseStamped global_pose; 330 if(!getRobotPose(global_pose, planner_costmap_ros_)){ 331 ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot"); 332 return false; 333 } 334 start = global_pose; 335 } 336 else 337 { 338 start = req.start; 339 } 340 341 //update the copy of the costmap the planner uses 342 clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_); 343 344 //制定规划策略 345 std::vector<geometry_msgs::PoseStamped> global_plan; 346 if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){ 347 ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance", 348 req.goal.pose.position.x, req.goal.pose.position.y); 349 350 //search outwards for a feasible goal within the specified tolerance在规定的公差范围内向外寻找可行的goal 351 geometry_msgs::PoseStamped p; 352 p = req.goal; 353 bool found_legal = false; 354 float resolution = planner_costmap_ros_->getCostmap()->getResolution(); 355 float search_increment = resolution*3.0;//以3倍分辨率的增量向外寻找 356 if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance; 357 for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) { 358 for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) { 359 for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) { 360 361 //不在本位置的外侧layer查找，太近的不找 362 if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue; 363 364 //从两个方向x、y查找精确的goal 365 for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) { 366 367 //第一次遍历如果偏移量过小,则去除这个点或者上一点 368 if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue; 369 370 for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) { 371 if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue; 372 373 p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult; 374 p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult; 375 376 if(planner_->makePlan(start, p, global_plan)){ 377 if(!global_plan.empty()){ 378 379 //adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there 380 //(the reachable goal should have been added by the global planner) 381 global_plan.push_back(req.goal); 382 383 found_legal = true; 384 ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y); 385 break; 386 } 387 } 388 else{ 389 ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y); 390 } 391 } 392 } 393 } 394 } 395 } 396 } 397 398 //copy the plan into a message to send out 399 resp.plan.poses.resize(global_plan.size()); 400 for(unsigned int i = 0; i < global_plan.size(); ++i){ 401 resp.plan.poses[i] = global_plan[i]; 402 } 403 404 return true; 405 } 406 //析构函数 407 MoveBase::~MoveBase(){ 408 recovery_behaviors_.clear(); 409 410 delete dsrv_; 411 412 if(as_ != NULL) 413 delete as_; 414 415 if(planner_costmap_ros_ != NULL) 416 delete planner_costmap_ros_; 417 418 if(controller_costmap_ros_ != NULL) 419 delete controller_costmap_ros_; 420 421 planner_thread_->interrupt(); 422 planner_thread_->join(); 423 424 delete planner_thread_; 425 426 delete planner_plan_; 427 delete latest_plan_; 428 delete controller_plan_; 429 430 planner_.reset(); 431 tc_.reset(); 432 } 433 434 bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){ 435 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex())); 436 437 //规划初始化 438 plan.clear(); 439 440 //激活句柄时调用 441 if(planner_costmap_ros_ == NULL) { 442 ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan"); 443 return false; 444 } 445 446 //得到机器人起始点 447 geometry_msgs::PoseStamped global_pose; 448 if(!getRobotPose(global_pose, planner_costmap_ros_)) { 449 ROS_WARN("Unable to get starting pose of robot, unable to create global plan"); 450 return false; 451 } 452 453 const geometry_msgs::PoseStamped& start = global_pose; 454 455 //如果规划失败或者返回一个长度为0的规划，则规划失败 456 if(!planner_->makePlan(start, goal, plan) || plan.empty()){ 457 ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y); 458 return false; 459 } 460 461 return true; 462 } 463 464 void MoveBase::publishZeroVelocity(){ 465 geometry_msgs::Twist cmd_vel; 466 cmd_vel.linear.x = 0.0; 467 cmd_vel.linear.y = 0.0; 468 cmd_vel.angular.z = 0.0; 469 vel_pub_.publish(cmd_vel); 470 } 471 472 bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){ 473 //1、首先检查四元数是否元素完整 474 if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){ 475 ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal"); 476 return false; 477 } 478 479 tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); 480 481 //2、检查四元数是否趋近于0 482 if(tf_q.length2() < 1e-6){ 483 ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal"); 484 return false; 485 } 486 487 //3、对四元数规范化，转化为vector 488 tf_q.normalize(); 489 490 tf2::Vector3 up(0, 0, 1); 491 492 double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle())); 493 494 if(fabs(dot - 1) > 1e-3){ 495 ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical."); 496 return false; 497 } 498 499 return true; 500 } 501 502 geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){ 503 std::string global_frame = planner_costmap_ros_->getGlobalFrameID(); 504 geometry_msgs::PoseStamped goal_pose, global_pose; 505 goal_pose = goal_pose_msg; 506 507 //tf一下 508 goal_pose.header.stamp = ros::Time(); 509 510 try{ 511 tf_.transform(goal_pose_msg, global_pose, global_frame); 512 } 513 catch(tf2::TransformException& ex){ 514 ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s", 515 goal_pose.header.frame_id.c_str(), global_frame.c_str(), ex.what()); 516 return goal_pose_msg; 517 } 518 519 return global_pose; 520 } 521 522 void MoveBase::wakePlanner(const ros::TimerEvent& event) 523 { 524 // we have slept long enough for rate 525 planner_cond_.notify_one(); 526 } 527 528 //planner线程的入口。这个函数需要等待actionlib服务器的cbMoveBase::executeCb来唤醒启动。 529 //主要作用是调用全局路径规划获取路径，同时保证规划的周期性以及规划超时清除goal 530 void MoveBase::planThread(){ 531 ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread..."); 532 ros::NodeHandle n; 533 ros::Timer timer; 534 bool wait_for_wake = false; 535 //1. 创建递归锁 536 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 537 while(n.ok()){ 538 //check if we should run the planner (the mutex is locked) 539 //2.判断是否阻塞线程 540 while(wait_for_wake || !runPlanner_){ 541 //if we should not be running the planner then suspend this thread 542 ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending"); 543 //当 std::condition_variable 对象的某个 wait 函数被调用的时候， 544 //它使用 std::unique_lock(通过 std::mutex) 来锁住当前线程。 545 //当前线程会一直被阻塞，直到另外一个线程在相同的 std::condition_variable 对象上调用了 notification 函数来唤醒当前线程。 546 planner_cond_.wait(lock); 547 wait_for_wake = false; 548 } 549 ros::Time start_time = ros::Time::now(); 550 551 //time to plan! get a copy of the goal and unlock the mutex 552 geometry_msgs::PoseStamped temp_goal = planner_goal_; 553 lock.unlock(); 554 ROS_DEBUG_NAMED("move_base_plan_thread","Planning..."); 555 556 //run planner 557 558 //3. 获取规划的全局路径 559 //这里的makePlan作用是获取机器人的位姿作为起点，然后调用全局规划器的makePlan返回规划路径，存储在planner_plan_ 560 planner_plan_->clear(); 561 bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_); 562 563 564 //4.如果获得了plan,则将其赋值给latest_plan_ 565 if(gotPlan){ 566 ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size()); 567 //pointer swap the plans under mutex (the controller will pull from latest_plan_) 568 std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_; 569 570 lock.lock(); 571 planner_plan_ = latest_plan_; 572 latest_plan_ = temp_plan; 573 last_valid_plan_ = ros::Time::now(); 574 planning_retries_ = 0; 575 new_global_plan_ = true; 576 577 ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner"); 578 579 //make sure we only start the controller if we still haven't reached the goal 580 if(runPlanner_) 581 state_ = CONTROLLING; 582 if(planner_frequency_ <= 0) 583 runPlanner_ = false; 584 lock.unlock(); 585 } 586 587 //5. 达到一定条件后停止路径规划,进入清障模式 588 //如果没有规划出路径，并且处于PLANNING状态，则判断是否超过最大规划周期或者规划次数 589 //如果是则进入自转模式，否则应该会等待MoveBase::executeCycle的唤醒再次规划 590 else if(state_==PLANNING){ 591 //仅在MoveBase::executeCb及其调用的MoveBase::executeCycle或者重置状态时会被设置为PLANNING 592 //一般是刚获得新目标，或者得到路径但计算不出下一步控制时重新进行路径规划 593 ROS_DEBUG_NAMED("move_base_plan_thread","No Plan..."); 594 ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_); 595 596 //check if we've tried to make a plan for over our time limit or our maximum number of retries 597 //issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_ 598 //is negative (the default), it is just ignored and we have the same behavior as ever 599 lock.lock(); 600 planning_retries_++; 601 if(runPlanner_ && 602 (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){ 603 //we'll move into our obstacle clearing mode 604 state_ = CLEARING; 605 runPlanner_ = false; // proper solution for issue #523 606 publishZeroVelocity(); 607 recovery_trigger_ = PLANNING_R; 608 } 609 610 lock.unlock(); 611 } 612 613 //take the mutex for the next iteration 614 lock.lock(); 615 616 617 //6.设置睡眠模式 618 //如果还没到规划周期则定时器睡眠，在定时器中断中通过planner_cond_唤醒，这里规划周期为0 619 if(planner_frequency_ > 0){ 620 ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now(); 621 if (sleep_time > ros::Duration(0.0)){ 622 wait_for_wake = true; 623 timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this); 624 } 625 } 626 } 627 } 628 629 void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal) 630 { 631 632 //1. 如果目标非法,返回 633 if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){ 634 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); 635 return; 636 } 637 638 //2. 将目标的坐标系统一转换为全局坐标系 639 geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose); 640 641 //3. 设置目标点并唤醒路径规划线程 642 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 643 planner_goal_ = goal; 644 runPlanner_ = true; 645 planner_cond_.notify_one(); 646 lock.unlock(); 647 648 current_goal_pub_.publish(goal); 649 std::vector<geometry_msgs::PoseStamped> global_plan; 650 651 ros::Rate r(controller_frequency_); 652 //4. 开启costmap更新 653 if(shutdown_costmaps_){ 654 ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously"); 655 planner_costmap_ros_->start(); 656 controller_costmap_ros_->start(); 657 } 658 659 //5. 重置时间标志位 660 last_valid_control_ = ros::Time::now(); 661 last_valid_plan_ = ros::Time::now(); 662 last_oscillation_reset_ = ros::Time::now(); 663 planning_retries_ = 0; 664 665 ros::NodeHandle n; 666 667 //6. 开启循环，循环判断是否有新的goal抢占 668 while(n.ok()) 669 { 670 671 //7. 修改循环频率 672 if(c_freq_change_) 673 { 674 ROS_INFO("Setting controller frequency to %.2f", controller_frequency_); 675 r = ros::Rate(controller_frequency_); 676 c_freq_change_ = false; 677 } 678 679 //8. 如果获得一个抢占式目标 680 if(as_->isPreemptRequested()){ 681 if(as_->isNewGoalAvailable()){ 682 //if we're active and a new goal is available, we'll accept it, but we won't shut anything down 683 move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal(); 684 685 //9.如果目标无效,则返回 686 if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){ 687 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); 688 return; 689 } 690 //10.将目标转换为全局坐标系 691 goal = goalToGlobalFrame(new_goal.target_pose); 692 693 //we'll make sure that we reset our state for the next execution cycle 694 695 //11.设置状态为PLANNING 696 recovery_index_ = 0; 697 state_ = PLANNING; 698 699 //we have a new goal so make sure the planner is awake 700 701 //12. 设置目标点并唤醒路径规划线程 702 lock.lock(); 703 planner_goal_ = goal; 704 runPlanner_ = true; 705 planner_cond_.notify_one(); 706 lock.unlock(); 707 708 //13. 把goal发布给可视化工具 709 ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y); 710 current_goal_pub_.publish(goal); 711 712 //make sure to reset our timeouts and counters 713 //14. 重置规划时间 714 last_valid_control_ = ros::Time::now(); 715 last_valid_plan_ = ros::Time::now(); 716 last_oscillation_reset_ = ros::Time::now(); 717 planning_retries_ = 0; 718 } 719 else { 720 721 //14.重置状态,设置为抢占式任务 722 //if we've been preempted explicitly we need to shut things down 723 resetState(); 724 725 //通知ActionServer已成功抢占 726 ROS_DEBUG_NAMED("move_base","Move base preempting the current goal"); 727 as_->setPreempted(); 728 729 //we'll actually return from execute after preempting 730 return; 731 } 732 } 733 734 //we also want to check if we've changed global frames because we need to transform our goal pose 735 //15.如果目标点的坐标系和全局地图的坐标系不相同 736 if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){ 737 //16,转换目标点坐标系 738 goal = goalToGlobalFrame(goal); 739 740 //we want to go back to the planning state for the next execution cycle 741 recovery_index_ = 0; 742 state_ = PLANNING; 743 744 //17. 设置目标点并唤醒路径规划线程 745 lock.lock(); 746 planner_goal_ = goal; 747 runPlanner_ = true; 748 planner_cond_.notify_one(); 749 lock.unlock(); 750 751 //publish the goal point to the visualizer 752 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); 753 current_goal_pub_.publish(goal); 754 755 //18.重置规划器相关时间标志位 756 last_valid_control_ = ros::Time::now(); 757 last_valid_plan_ = ros::Time::now(); 758 last_oscillation_reset_ = ros::Time::now(); 759 planning_retries_ = 0; 760 } 761 762 //for timing that gives real time even in simulation 763 ros::WallTime start = ros::WallTime::now(); 764 765 //19. 到达目标点的真正工作，控制机器人进行跟随 766 bool done = executeCycle(goal, global_plan); 767 768 //20.如果完成任务则返回 769 if(done) 770 return; 771 772 //check if execution of the goal has completed in some way 773 774 ros::WallDuration t_diff = ros::WallTime::now() - start; 775 ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec()); 776 //21. 执行休眠动作 777 r.sleep(); 778 //make sure to sleep for the remainder of our cycle time 779 if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING) 780 ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec()); 781 } 782 783 //22. 唤醒计划线程，以便它可以干净地退出 784 lock.lock(); 785 runPlanner_ = true; 786 planner_cond_.notify_one(); 787 lock.unlock(); 788 789 //23. 如果节点结束就终止并返回 790 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed"); 791 return; 792 } 793 794 double MoveBase::distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) 795 { 796 return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y); 797 } 798 799 //两个参数分别是目标点位姿以及规划出的全局路径.通过这两个参数，实现以下功能： 800 //利用局部路径规划器直接输出轮子速度，控制机器人按照路径走到目标点，成功返回真，否则返回假。在actionlib server的回调MoveBase::executeCb中被调用。 801 bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){ 802 803 boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); 804 //we need to be able to publish velocity commands 805 geometry_msgs::Twist cmd_vel; 806 807 808 //1.获取机器人当前位置 809 geometry_msgs::PoseStamped global_pose; 810 getRobotPose(global_pose, planner_costmap_ros_); 811 const geometry_msgs::PoseStamped& current_position = global_pose; 812 813 //push the feedback out 814 move_base_msgs::MoveBaseFeedback feedback; 815 feedback.base_position = current_position; 816 as_->publishFeedback(feedback); 817 818 //2.重置震荡标志位 819 if(distance(current_position, oscillation_pose_) >= oscillation_distance_) 820 { 821 last_oscillation_reset_ = ros::Time::now(); 822 oscillation_pose_ = current_position; 823 824 //if our last recovery was caused by oscillation, we want to reset the recovery index 825 if(recovery_trigger_ == OSCILLATION_R) 826 recovery_index_ = 0; 827 } 828 829 //3.地图数据超时，即观测传感器数据不够新,停止机器人,返回false 830 if(!controller_costmap_ros_->isCurrent()){ 831 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()); 832 publishZeroVelocity(); 833 return false; 834 } 835 836 //4. 如果获取新的全局路径,则将它传输给控制器，完成latest_plan_到controller_plan_的转换 837 if(new_global_plan_){ 838 //make sure to set the new plan flag to false 839 new_global_plan_ = false; 840 841 ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers"); 842 843 //do a pointer swap under mutex 844 std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_; 845 846 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 847 controller_plan_ = latest_plan_; 848 latest_plan_ = temp_plan; 849 lock.unlock(); 850 ROS_DEBUG_NAMED("move_base","pointers swapped!"); 851 852 //5. 给控制器设置全局路径 853 if(!tc_->setPlan(*controller_plan_)){ 854 //ABORT and SHUTDOWN COSTMAPS 855 ROS_ERROR("Failed to pass global plan to the controller, aborting."); 856 resetState(); 857 858 //disable the planner thread 859 lock.lock(); 860 runPlanner_ = false; 861 lock.unlock(); 862 //6.设置动作中断,返回true 863 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller."); 864 return true; 865 } 866 867 //如果全局路径有效，则不需要recovery 868 if(recovery_trigger_ == PLANNING_R) 869 recovery_index_ = 0; 870 } 871 872 873 //5. move_base 状态机，处理导航的控制逻辑 874 //一般默认状态或者接收到一个有效goal时是PLANNING，在规划出全局路径后state_会由PLANNING->CONTROLLING 875 //如果规划失败则由PLANNING->CLEARING。 876 switch(state_){ 877 //6. 机器人规划状态,尝试获取一条全局路径 878 case PLANNING: 879 { 880 boost::recursive_mutex::scoped_lock lock(planner_mutex_); 881 runPlanner_ = true; 882 planner_cond_.notify_one();//还在PLANNING中则唤醒规划线程让它干活 883 } 884 ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state."); 885 break; 886 887 //7.机器人控制状态,尝试获取一个有效的速度命令 888 case CONTROLLING: 889 ROS_DEBUG_NAMED("move_base","In controlling state."); 890 891 //8.如果到达目标点,重置状态,设置动作成功,返回true 892 if(tc_->isGoalReached()){ 893 ROS_DEBUG_NAMED("move_base","Goal reached!"); 894 resetState(); 895 896 //disable the planner thread 897 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 898 runPlanner_ = false; 899 lock.unlock(); 900 901 as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached."); 902 return true; 903 } 904 905 //9. 如果超过震荡时间,停止机器人,设置清障标志位 906 if(oscillation_timeout_ > 0.0 && 907 last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now()) 908 { 909 publishZeroVelocity(); 910 state_ = CLEARING; 911 recovery_trigger_ = OSCILLATION_R; 912 } 913 914 { 915 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex())); 916 //10. 获取有效速度,如果获取成功，直接发布到cmd_vel 917 if(tc_->computeVelocityCommands(cmd_vel)){ 918 ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf", 919 cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z ); 920 last_valid_control_ = ros::Time::now(); 921 //make sure that we send the velocity command to the base 922 vel_pub_.publish(cmd_vel); 923 if(recovery_trigger_ == CONTROLLING_R) 924 recovery_index_ = 0; 925 } 926 else { 927 //11.如果没有获取到有效速度 928 ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan."); 929 ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_); 930 931 //12.判断超过尝试时间,如果超时，停止机器人,进入清障模式 932 if(ros::Time::now() > attempt_end){ 933 //we'll move into our obstacle clearing mode 934 publishZeroVelocity(); 935 state_ = CLEARING; 936 recovery_trigger_ = CONTROLLING_R; 937 } 938 else{ 939 //如果不超时，让全局再规划一个路径 940 last_valid_plan_ = ros::Time::now(); 941 planning_retries_ = 0; 942 state_ = PLANNING; 943 publishZeroVelocity(); 944 945 //enable the planner thread in case it isn't running on a clock 946 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 947 runPlanner_ = true; 948 planner_cond_.notify_one(); 949 lock.unlock(); 950 } 951 } 952 } 953 954 break; 955 956 //13. 规划或者控制失败，恢复或者进入到清障模式 957 case CLEARING: 958 ROS_DEBUG_NAMED("move_base","In clearing/recovery state"); 959 960 //14. 有可用恢复器,执行恢复动作,设置状态为PLANNING 961 if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){ 962 ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size()); 963 recovery_behaviors_[recovery_index_]->runBehavior(); 964 965 //we at least want to give the robot some time to stop oscillating after executing the behavior 966 last_oscillation_reset_ = ros::Time::now(); 967 968 //we'll check if the recovery behavior actually worked 969 ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state"); 970 last_valid_plan_ = ros::Time::now(); 971 planning_retries_ = 0; 972 state_ = PLANNING; 973 974 //update the index of the next recovery behavior that we'll try 975 recovery_index_++; 976 } 977 else{ 978 979 //15.没有可用恢复器,结束动作,返回true 980 ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it."); 981 //disable the planner thread 982 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 983 runPlanner_ = false; 984 lock.unlock(); 985 986 ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this."); 987 988 if(recovery_trigger_ == CONTROLLING_R){ 989 ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors"); 990 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors."); 991 } 992 else if(recovery_trigger_ == PLANNING_R){ 993 ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors"); 994 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors."); 995 } 996 else if(recovery_trigger_ == OSCILLATION_R){ 997 ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors"); 998 as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors."); 999 } 1000 resetState(); 1001 return true; 1002 } 1003 break; 1004 default: 1005 ROS_ERROR("This case should never be reached, something is wrong, aborting"); 1006 resetState(); 1007 //disable the planner thread 1008 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 1009 runPlanner_ = false; 1010 lock.unlock(); 1011 as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it."); 1012 return true; 1013 } 1014 1015 //we aren't done yet 1016 return false; 1017 } 1018 1019 //recovery是指恢复的规划器，其跟全局规划器和局部规划器是同一个等级的。 1020 //不同的是，它是在机器人在局部代价地图或者全局代价地图中找不到路时才会被调用，比如rotate_recovery让机器人原地360°旋转，clear_costmap_recovery将代价地图恢复到静态地图的样子。 1021 bool MoveBase::loadRecoveryBehaviors(ros::NodeHandle node){ 1022 XmlRpc::XmlRpcValue behavior_list; 1023 if(node.getParam("recovery_behaviors", behavior_list)){ 1024 if(behavior_list.getType() == XmlRpc::XmlRpcValue::TypeArray){ 1025 for(int i = 0; i < behavior_list.size(); ++i){ 1026 if(behavior_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct){ 1027 if(behavior_list[i].hasMember("name") && behavior_list[i].hasMember("type")){ 1028 //check for recovery behaviors with the same name 1029 for(int j = i + 1; j < behavior_list.size(); j++){ 1030 if(behavior_list[j].getType() == XmlRpc::XmlRpcValue::TypeStruct){ 1031 if(behavior_list[j].hasMember("name") && behavior_list[j].hasMember("type")){ 1032 std::string name_i = behavior_list[i]["name"]; 1033 std::string name_j = behavior_list[j]["name"]; 1034 if(name_i == name_j){ 1035 ROS_ERROR("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.", 1036 name_i.c_str()); 1037 return false; 1038 } 1039 } 1040 } 1041 } 1042 } 1043 else{ 1044 ROS_ERROR("Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead."); 1045 return false; 1046 } 1047 } 1048 else{ 1049 ROS_ERROR("Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We'll use the default recovery behaviors instead.", 1050 behavior_list[i].getType()); 1051 return false; 1052 } 1053 } 1054 1055 //if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors 1056 for(int i = 0; i < behavior_list.size(); ++i){ 1057 try{ 1058 //check if a non fully qualified name has potentially been passed in 1059 if(!recovery_loader_.isClassAvailable(behavior_list[i]["type"])){ 1060 std::vector<std::string> classes = recovery_loader_.getDeclaredClasses(); 1061 for(unsigned int i = 0; i < classes.size(); ++i){ 1062 if(behavior_list[i]["type"] == recovery_loader_.getName(classes[i])){ 1063 //if we've found a match... we'll get the fully qualified name and break out of the loop 1064 ROS_WARN("Recovery behavior specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.", 1065 std::string(behavior_list[i]["type"]).c_str(), classes[i].c_str()); 1066 behavior_list[i]["type"] = classes[i]; 1067 break; 1068 } 1069 } 1070 } 1071 1072 boost::shared_ptr<nav_core::RecoveryBehavior> behavior(recovery_loader_.createInstance(behavior_list[i]["type"])); 1073 1074 //shouldn't be possible, but it won't hurt to check 1075 if(behavior.get() == NULL){ 1076 ROS_ERROR("The ClassLoader returned a null pointer without throwing an exception. This should not happen"); 1077 return false; 1078 } 1079 1080 //initialize the recovery behavior with its name 1081 behavior->initialize(behavior_list[i]["name"], &tf_, planner_costmap_ros_, controller_costmap_ros_); 1082 recovery_behaviors_.push_back(behavior); 1083 } 1084 catch(pluginlib::PluginlibException& ex){ 1085 ROS_ERROR("Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what()); 1086 return false; 1087 } 1088 } 1089 } 1090 else{ 1091 ROS_ERROR("The recovery behavior specification must be a list, but is of XmlRpcType %d. We'll use the default recovery behaviors instead.", 1092 behavior_list.getType()); 1093 return false; 1094 } 1095 } 1096 else{ 1097 //if no recovery_behaviors are specified, we'll just load the defaults 1098 return false; 1099 } 1100 1101 //if we've made it here... we've constructed a recovery behavior list successfully 1102 return true; 1103 } 1104 1105 //we'll load our default recovery behaviors here 1106 void MoveBase::loadDefaultRecoveryBehaviors(){ 1107 recovery_behaviors_.clear(); 1108 try{ 1109 //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility 1110 ros::NodeHandle n("~"); 1111 n.setParam("conservative_reset/reset_distance", conservative_reset_dist_); 1112 n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4); 1113 1114 //1、加载recovery behavior清理costmap 1115 boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery")); 1116 cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_); 1117 recovery_behaviors_.push_back(cons_clear); 1118 1119 //2、加载recovery behavior 原地旋转 1120 boost::shared_ptr<nav_core::RecoveryBehavior> rotate(recovery_loader_.createInstance("rotate_recovery/RotateRecovery")); 1121 if(clearing_rotation_allowed_){ 1122 rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_); 1123 recovery_behaviors_.push_back(rotate); 1124 } 1125 1126 //3、加载 recovery behavior 重置 costmap 1127 boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery")); 1128 ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_); 1129 recovery_behaviors_.push_back(ags_clear); 1130 1131 //4、再一次旋转 1132 if(clearing_rotation_allowed_) 1133 recovery_behaviors_.push_back(rotate); 1134 } 1135 catch(pluginlib::PluginlibException& ex){ 1136 ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what()); 1137 } 1138 1139 return; 1140 } 1141 1142 void MoveBase::resetState(){ 1143 // Disable the planner thread 1144 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 1145 runPlanner_ = false; 1146 lock.unlock(); 1147 1148 // Reset statemachine 1149 state_ = PLANNING; 1150 recovery_index_ = 0; 1151 recovery_trigger_ = PLANNING_R; 1152 publishZeroVelocity(); 1153 1154 //if we shutdown our costmaps when we're deactivated... we'll do that now 1155 if(shutdown_costmaps_){ 1156 ROS_DEBUG_NAMED("move_base","Stopping costmaps"); 1157 planner_costmap_ros_->stop(); 1158 controller_costmap_ros_->stop(); 1159 } 1160 } 1161 1162 bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap) 1163 { 1164 tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose); 1165 geometry_msgs::PoseStamped robot_pose; 1166 tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose); 1167 robot_pose.header.frame_id = robot_base_frame_; 1168 robot_pose.header.stamp = ros::Time(); // latest available 1169 ros::Time current_time = ros::Time::now(); // save time for checking tf delay later 1170 1171 // 转换到统一的全局坐标 1172 try 1173 { 1174 tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID()); 1175 } 1176 catch (tf2::LookupException& ex) 1177 { 1178 ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what()); 1179 return false; 1180 } 1181 catch (tf2::ConnectivityException& ex) 1182 { 1183 ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what()); 1184 return false; 1185 } 1186 catch (tf2::ExtrapolationException& ex) 1187 { 1188 ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what()); 1189 return false; 1190 } 1191 1192 // 全局坐标时间戳是否在costmap要求下 1193 if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance()) 1194 { 1195 ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " \ 1196 "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(), 1197 current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance()); 1198 return false; 1199 } 1200 1201 return true; 1202 } 1203 };
博文主要是总结自己的学习，因此有很多知识点没有提到，仅仅提了个人比较容易遗忘的或者非常重要的知识点。很多资料来源于网络和对一些课程的整理，侵权删。格式没花精力调整，望谅解。
