一、概述
目测是全网最全的解析，花了几个小时通读并整理的，供大家参考学习。
概况的话可以看下古月居 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 };
来源：oschina
链接：https://my.oschina.net/u/4256916/blog/3311726