一、概述
详见古月居 https://www.guyuehome.com/270，其实是翻译官方的，感兴趣的也可以去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