Navigation（⼀）move_ba源码最全解析
⼀、概述
⽬测是全⽹最全的解析，花了⼏个⼩时通读并整理的，供⼤家参考学习。
本篇是直接源码配注释的，所以逻辑性不够强，我还写了⼀篇按照代码执⾏逻辑读代码的⽂章，个⼈认为⽐这篇有⽤得多，以下为链接，可以配合着看：。 概况的话可以看下古⽉居，其实它是翻译官⽅的，英语ok的可以去ros wiki翻看原版。
重点：navfn包全局规划（Dji算法）、ba_local_planner包局部规划（Trajectory Rollout 和Dynamic Window approaches算法）
⼆、move_ba.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/rver/simple_action_rver.h>
10 #include <move_ba_msgs/MoveBaAction.h>
11
12 #include <nav_core/ba_local_planner.h>
13 #include <nav_core/ba_global_planner.h>
14 #include <nav_core/recovery_behavior.h>
15 #include <geometry_msgs/PoStamped.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/rver.h>
24 #include "move_ba/MoveBaConfig.h"
25
26namespace move_ba {
27//声明rver端，消息类型是move_ba_msgs::MoveBaAction
28 typedef actionlib::SimpleActionServer<move_ba_msgs::MoveBaAction> MoveBaActionServer;
29//moveba状态表⽰
30enum MoveBaState {
31 PLANNING,
32 CONTROLLING,
妆前乳的作用
33 CLEARING
34 };
35//触发恢复模式
36enum RecoveryTrigger
37 {
38 PLANNING_R,
39 CONTROLLING_R,
40 OSCILLATION_R
41 };
42
43//使⽤actionlib：：ActionServer接⼝的类，该接⼝将robot移动到⽬标位置。
44class MoveBa {
45public:
46// 构造函数，传⼊的参数是tf
47 MoveBa(tf2_ros::Buffer& tf);
48
49//析构函数
50virtual ~MoveBa();
51
52//控制闭环、全局规划、到达⽬标返回true，没有到达返回fal
53bool executeCycle(geometry_msgs::PoStamped& goal, std::vector<geometry_msgs::PoStamped>& global_plan);
54
55private:
56/**
57 * @brief 清除costmap的rver端
58 * @param req 表⽰rver的request
59 * @param resp 表⽰rver的respon
60 * @return 如果rver端被成功调⽤则为True，否则fal
61*/
62bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Respon &resp);
63
64/**
65 * @brief 当action不活跃时，调⽤此函数，返回plan
66 * @param req 表⽰goal的request
67 * @param resp 表⽰plan的request
68 * @return 规划成功返回reue，否则返回fal
69*/
70bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Respon &resp);
71
72/**
73 * @brief 新的全局规划
74 * @param goal 规划的⽬标点
75 * @param plan 规划
76 * @return 规划成功则返回True 否则fal
77*/
78bool makePlan(const geometry_msgs::PoStamped& goal, std::vector<geometry_msgs::PoStamped>& plan);
79
80/**
81 * @brief 从参数服务器加载导航参数Load the recovery behaviors
82 * @param node 表⽰ ros::NodeHandle 加载的参数
83 * @return 加载成功返回True 否则 fal
84*/
85bool loadRecoveryBehaviors(ros::NodeHandle node);
86
87// 加载默认导航参数
88void loadDefaultRecoveryBehaviors();
89
90/**
91 * @brief 清楚机器⼈局部规划框的障碍物
92 * @param size_x 局部规划框的长x
93 * @param size_y 局部规划框的宽y
94*/
95void clearCostmapWindows(double size_x, double size_y);
96
97//发布速度为0的指令
98void publishZeroVelocity();
99
100// 重置move_ba action的状态，设置速度为0
101void retState();
102动漫专业学院
103void goalCB(const geometry_msgs::PoStamped::ConstPtr& goal);
104
105void planThread();
106
107void executeCb(const move_ba_msgs::MoveBaGoalConstPtr& move_ba_goal);
108
109bool isQuaternionValid(const geometry_msgs::Quaternion& q);
110
111bool getRobotPo(geometry_msgs::PoStamped& global_po, costmap_2d::Costmap2DROS* costmap); 112
113double distance(const geometry_msgs::PoStamped& p1, const geometry_msgs::PoStamped& p2);
114
115 geometry_msgs::PoStamped goalToGlobalFrame(const geometry_msgs::PoStamped& goal_po_msg); 116
117//周期性地唤醒规划器
118void wakePlanner(const ros::TimerEvent& event);
119空腹喝纯牛奶
120 tf2_ros::Buffer& tf_;
121
122 MoveBaActionServer* as_; //就是actionlib的rver端
123
124 boost::shared_ptr<nav_core::BaLocalPlanner> tc_;//局部规划器，加载并创建实例后的指针
125 costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;//costmap的实例化指针126
127 boost::shared_ptr<nav_core::BaGlobalPlanner> planner_;//全局规划器，加载并创建实例后的指针
128 std::string robot_ba_frame_, global_frame_;
129
130 std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;//可能是出错后的恢复131 unsigned int recovery_index_;
132
133 geometry_msgs::PoStamped global_po_;
134double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
135double planner_patience_, controller_patience_;
136 int32_t max_planning_retries_;
137 uint32_t planning_retries_;
138double conrvative_ret_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_;
142bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_;
143double oscillation_timeout_, oscillation_distance_;
144
145 MoveBaState state_;
146 RecoveryTrigger recovery_trigger_;
147
148 ros::Time last_valid_plan_, last_valid_control_, last_oscillation_ret_;
149 geometry_msgs::PoStamped oscillation_po_;
150 pluginlib::ClassLoader<nav_core::BaGlobalPlanner> bgp_loader_;
151 pluginlib::ClassLoader<nav_core::BaLocalPlanner> blp_loader_;
152 pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;
153
154//触发哪种规划器
155 std::vector<geometry_msgs::PoStamped>* planner_plan_;//保存最新规划的路径，传给latest_plan_
156 std::vector<geometry_msgs::PoStamped>* latest_plan_;//在executeCycle中传给controller_plan_
157 std::vector<geometry_msgs::PoStamped>* controller_plan_;
158
159//规划器线程
160bool runPlanner_;
161 boost::recursive_mutex planner_mutex_;
162 boost::condition_variable_any planner_cond_;
163 geometry_msgs::PoStamped planner_goal_;
164 boost::thread* planner_thread_;
165
166
167 boost::recursive_mutex configuration_mutex_;
168 dynamic_reconfigure::Server<move_ba::MoveBaConfig> *dsrv_;
169
170void reconfigureCB(move_ba::MoveBaConfig &config, uint32_t level);
171
172 move_ba::MoveBaConfig last_config_;
173 move_ba::MoveBaConfig default_config_;
174bool tup_, p_freq_change_, c_freq_change_;
175bool new_global_plan_;
176 };
177 };
178#endif
三、move_ba_node.cpp
1 #include <move_ba/move_ba.h>
2 #include <tf2_ros/transform_listener.h>
3
4int main(int argc, char** argv){
5 ros::init(argc, argv, "move_ba_node");
6 tf2_ros::Buffer buffer(ros::Duration(10));
7 tf2_ros::TransformListener tf(buffer);
8
9 move_ba::MoveBa move_ba( buffer );//本cpp中只调⽤了这个构造函数
10
11//ros::MultiThreadedSpinner s;
12 ros::spin();
13
14return(0);
15 }
四、move_ba.cpp
1 #include <move_ba/move_ba.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
11namespace move_ba {
12
13 MoveBa::MoveBa(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::BaGlobalPlanner"),
18 blp_loader_("nav_core", "nav_core::BaLocalPlanner"),
19 recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
20 planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
21 runPlanner_(fal), tup_(fal), p_freq_change_(fal), c_freq_change_(fal), new_global_plan_(fal) {
22
23
24//1. 创建move_ba action，绑定回调函数。定义⼀个名为move_ba的SimpleActionServer。该服务器的Callback为MoveBa::executeCb。
25 as_ = new MoveBaActionServer(ros::NodeHandle(), "move_ba", boost::bind(&MoveBa::executeCb, this, _1), fal);
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("ba_global_planner", global_planner, std::string("navfn/NavfnROS"));
36 private_nh.param("ba_local_planner", local_planner, std::string("ba_local_planner/TrajectoryPlannerROS"));
37 private_nh.param("global_costmap/robot_ba_frame", robot_ba_frame_, std::string("ba_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//t up plan triple buffer
50 planner_plan_ = new std::vector<geometry_msgs::PoStamped>();
51 latest_plan_ = new std::vector<geometry_msgs::PoStamped>();
52 controller_plan_ = new std::vector<geometry_msgs::PoStamped>();
53
54//3. 设置规划器线程
55//t up the planner's thread
56 planner_thread_ = new boost::thread(boost::bind(&MoveBa::planThread, this));
57
58//4. 创建发布者
59//for commanding the ba
60 vel_pub_ = nh.adverti<geometry_msgs::Twist>("cmd_vel", 1);
61 current_goal_pub_ = private_nh.adverti<geometry_msgs::PoStamped>("current_goal", 0 );
62
63 ros::NodeHandle action_nh("move_ba");
北岛64 action_goal_pub_ = action_nh.adverti<move_ba_msgs::MoveBaActionGoal>("goal", 1);
65
66//提供消息类型为geometry_msgs::PoStamped的发送goals的接⼝，⽐如cb为MoveBa::goalCB，在rviz中输⼊的⽬标点就是通过这个函数来响应的
67 ros::NodeHandle simple_nh("move_ba_simple");
68 goal_sub_ = simple_nh.subscribe<geometry_msgs::PoStamped>("goal", 1, boost::bind(&MoveBa::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("conrvative_ret_dist", conrvative_ret_dist_, 3.0);
75
76 private_nh.param("shutdown_costmaps", shutdown_costmaps_, fal);
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' and initializer a pointer we'll u with the underlying map
镜片选择82 planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
83 planner_costmap_ros_->pau();
84
85//initialize the global planner
86try {
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' and initializer a pointer we'll u with the underlying map
96 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
97 controller_costmap_ros_->pau();
98
99//create a local planner
100try {
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 bad on nsor data
110
111//7.开始更新costmap
112 planner_costmap_ros_->start();
113 controller_costmap_ros_->start();
114
115//adverti a rvice for getting a plan
116//8.开启创建地图，清除地图服务
117 make_plan_srv_ = private_nh.advertiService("make_plan", &MoveBa::planService, this);
118
119//定义⼀个名为clear_costmaps的服务，cb为MoveBa::clearCostmapsService
120 clear_costmaps_srv_ = private_nh.advertiService("clear_costmaps", &MoveBa::clearCostmapsService, this);
121
122//如果不⼩⼼关闭了costmap
123if(shutdown_costmaps_){
124 ROS_DEBUG_NAMED("move_ba","Stopping costmaps initially");
125 planner_costmap_ros_->stop();
126 controller_costmap_ros_->stop();
127 }
128
129//9.加载指定的恢复器
130if(!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_ba动作器
141 as_->start();
142//启动动态参数服务器，回调函数为reconfigureCB，推荐看⼀下古⽉居/1173
143 dsrv_ = new dynamic_reconfigure::Server<move_ba::MoveBaConfig>(ros::NodeHandle("~"));
144 dynamic_reconfigure::Server<move_ba::MoveBaConfig>::CallbackType cb = boost::bind(
&MoveBa::reconfigureCB, this, _1, _2);
145 dsrv_->tCallback(cb);
146 }
147
148void MoveBa::reconfigureCB(move_ba::MoveBaConfig &config, uint32_t level){
149 boost::recursive_mutex::scoped_lock l(configuration_mutex_);
150
151//⼀旦被调⽤，我们要确保有原始配置
152if(!tup_)
153 {
154 last_config_ = config;
155 default_config_ = config;
156 tup_ = true;
157return;
158 }
159
store_defaults) {
161 config = default_config_;
162//如果有⼈在参数服务器上设置默认值，要防⽌循环
163 store_defaults = fal;
164 }
165
166if(planner_frequency_ != config.planner_frequency)
167 {
168 planner_frequency_ = config.planner_frequency;
169 p_freq_change_ = true;
170 }
171
172if(controller_frequency_ != ller_frequency)
173 {
174 controller_frequency_ = ller_frequency;
175 c_freq_change_ = true;
176 }
177
178 planner_patience_ = config.planner_patience;
179 controller_patience_ = ller_patience;
180 max_planning_retries_ = config.max_planning_retries;
181 conrvative_ret_dist_ = vative_ret_dist;
182
183 recovery_behavior_enabled_ = very_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;
189if(config.ba_global_planner != last_config_.ba_global_planner) {
190 boost::shared_ptr<nav_core::BaGlobalPlanner> old_planner = planner_;
191//创建全局规划
192 ROS_INFO("Loading global planner %s", config.ba_global_planner.c_str());
193try {
194 planner_ = bgp_loader_.createInstance(config.ba_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 retState();
204 planner_->initialize(bgp_loader_.getName(config.ba_global_planner), planner_costmap_ros_);
205
206lock.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.ba_global_planner.c_str(), ex.what());
210 planner_ = old_planner;
211 config.ba_global_planner = last_config_.ba_global_planner;
212 }
213 }
214
215if(config.ba_local_planner != last_config_.ba_local_planner){
216 boost::shared_ptr<nav_core::BaLocalPlanner> old_planner = tc_;
217//创建局部规划
218try {
219 tc_ = blp_loader_.createInstance(config.ba_local_planner);
220// 清理旧的
221 planner_plan_->clear();
222 latest_plan_->clear();
223 controller_plan_->clear();
224 retState();
225 tc_->initialize(blp_loader_.getName(config.ba_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.ba_local_planner.c_str(), ex.what());
229 tc_ = old_planner;
230 config.ba_local_planner = last_config_.ba_local_planner;
231 }
232 }
233
234 last_config_ = config;
235 }
236
237//为rviz等提供调⽤借⼝，将geometry_msgs::PoStamped形式的goal转换成move_ba_msgs::MoveBaActionGoal，再发布到对应类型的goal话题中 238void MoveBa::goalCB(const geometry_msgs::PoStamped::ConstPtr& goal){
239 ROS_DEBUG_NAMED("move_ba","In ROS goal callback, wrapping the PoStamped in th
e action message and re-nding to the rver.");
240 move_ba_msgs::MoveBaActionGoal action_goal;
241 action_goal.header.stamp = ros::Time::now();
242 al.target_po = *goal;
243
244 action_goal_pub_.publish(action_goal);
245 }
246
元胡止痛胶囊247void MoveBa::clearCostmapWindows(double size_x, double size_y){
248 geometry_msgs::PoStamped global_po;
249
250//clear the planner's costmap
251 getRobotPo(global_po, planner_costmap_ros_);
252
253 std::vector<geometry_msgs::Point> clear_poly;
254double x = global_po.po.position.x;
255double y = global_po.po.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()->tConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
275
276//clear the controller's costmap
277 getRobotPo(global_po, controller_costmap_ros_);
278
279 clear_poly.clear();
280 x = global_po.po.position.x;
281 y = global_po.po.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()->tConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);
300 }
301
302bool MoveBa::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Respon &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_->retLayers();
306
307 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner(*(planner_costmap_ros_->getCostmap()->getMutex()));
308 planner_costmap_ros_->retLayers();
309return true;
310 }
311
312
313bool MoveBa::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Respon
&resp){
314if(as_->isActive()){
315 ROS_ERROR("move_ba must be in an inactive state to make a plan for an external ur");
316return fal;
317 }
318//make sure we have a costmap for our planner
319if(planner_costmap_ros_ == NULL){
320 ROS_ERROR("move_ba cannot make a plan for you becau it doesn't have a costmap");
321return fal;
322 }
323
324//1. 获取起始点
325 geometry_msgs::PoStamped start;
326//如果起始点为空，设置global_po为起始点
327if(req.start.header.pty())
328 {
329 geometry_msgs::PoStamped global_po;
330if(!getRobotPo(global_po, planner_costmap_ros_)){
331 ROS_ERROR("move_ba cannot make a plan for you becau it could not get the start po of the robot");
332return fal;
333 }
