Navigation源码阅读（一） move_base

  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