ROS movebase集解

目录

• 01.move_base.h头文件

 

---

回到顶部

01.move_base.h头文件

namespace move_base
{
    // 这是一个actionServer服务器，消息类型：move_base_msgs::MoveBaseAction
    typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;

    enum MoveBaseState // movebase在导航过程中，实时的状态
    {
        PLANNING,
        CONTROLLING,
        CLEARING
    };

    class MoveBase
    {
    public:
        MoveBase(tf2_ros::Buffer &tf); // 构造函数，接收一个tfBuffer
        virtual ~MoveBase();           // 析构函数，虚函数
        bool executeCycle(geometry_msgs::PoseStamped &goal);

    private:
        // 这是一个服务，clear 障碍代价地图
        bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
        // 这是一个服务，当action Server失效的时候，调用该服务将会返回一个路径规划；
        bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
        // 制定一个全局规划路径，包含一系列的带时间做带时间戳的位姿点
        bool makePlan(const geometry_msgs::PoseStamped &goal, std::vector<geometry_msgs::PoseStamped> &plan);

        bool loadRecoveryBehaviors(ros::NodeHandle node);
        void loadDefaultRecoveryBehaviors();
        // Clears obstacles within a window around the robot
        void clearCostmapWindows(double size_x, double size_y);

        void publishZeroVelocity();

        void resetState();
        /* ---------------------------------- goalCb() ------------------------------- */
        //  goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
        // 01.这是"goal"话题订阅的回调函数，在构造函数中可以看到其关联的subscriber；处理geometry::PoseStamped消息
        // 02.In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server 
        //     该回调函数功能是将PoseStamped消息打包成action message消息，然后再通过【action_goal_pub_】发布器，将action消息发送给ActionServer
        void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal);

        void planThread();
        /* ---------------------------------- executeCb() ------------------------------- */
        // ActionServer的callback函数，当收到MoveBaseGoal的运动目标请求的时候，进入的回调处理函数；是主要的处理函数
        void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal);
        // 四元数是否有效
        bool isQuaternionValid(const geometry_msgs::Quaternion &q);
        // ？？？？？？？？？？ 代价地图与位姿之间的关系，   根据方法名称：getRobotPose
        bool getRobotPose(geometry_msgs::PoseStamped &global_pose, costmap_2d::Costmap2DROS *costmap);
        // 两个pose之间的距离
        double distance(const geometry_msgs::PoseStamped &p1, const geometry_msgs::PoseStamped &p2);
        // 把一个目标点转换到全局坐标系下；
        geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped &goal_pose_msg);
        /* This is used to wake the planner at periodic intervals.   这是一个定时器，每隔一段时间，执行响应的操作*/
        void wakePlanner(const ros::TimerEvent &event);
        
        // --------------------------------------------------------------------
        // -------------------------- 数据成员 ----------------------------------
        // --------------------------------------------------------------------
        // --------------------------------------------------------------------
        tf2_ros::Buffer &tf_;       // 用于存储坐标变换
        MoveBaseActionServer *as_;      // actionserver 指针；

        // 发布器：currentGoal  velpub， actionGoalPub  RecoveryStatus
        ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_, recovery_status_pub_;
        // 订阅goal
        ros::Subscriber goal_sub_;
        // 两个服务，make_plan clear_costmaps
        ros::ServiceServer make_plan_srv_, clear_costmaps_srv_;

        
        // planner_ 为一个智能指针，指向一个全局规划器：nav_core::BaseGlobalPlanner
        boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_; 
        // tc_  为一个智能指针，指向一个局部规划器 nav_core::BaseLocalPlanner
        boost::shared_ptr<nav_core::BaseLocalPlanner> tc_; 
        // planner_costmap_ros_ 用于构造全局代价地图
        // controller_costmap_ros_ 用于构造局部代价地图， 这两个的地图类型都为：costmap_2d::Costmap2DROS
        costmap_2d::Costmap2DROS *planner_costmap_ros_, *controller_costmap_ros_;

        // 机器人坐标系和全局坐标系的名称；        
        std::string robot_base_frame_, global_frame_;

        // 用于规划失败后的恢复行为； vector中存储一些列的指针，每个指针指向nav_core::RecoveryBehavior 
        std::vector<boost::shared_ptr<nav_core::RecoveryBehavior>> recovery_behaviors_;
        std::vector<std::string> recovery_behavior_names_;
        unsigned int recovery_index_;
        // 变量，全局位姿
        geometry_msgs::PoseStamped global_pose_;
        geometry_msgs::PoseStamped oscillation_pose_;
        geometry_msgs::PoseStamped planner_goal_;

        double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_;
        double planner_patience_, controller_patience_;
        int32_t max_planning_retries_;
        uint32_t planning_retries_;
        double conservative_reset_dist_, clearing_radius_;
        
        bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_;
        bool make_plan_clear_costmap_, make_plan_add_unreachable_goal_;
        double oscillation_timeout_, oscillation_distance_;

        MoveBaseState state_;
        RecoveryTrigger recovery_trigger_;
        ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_;
        
        // pluginlib::ClassLoader 是属于一个模板类ClassLoader，实例化为BaseGlobalPlanner或者BaseLocalPlanner。关于pluginlib的分析也有在接下来的文章中进行。
        // bgp_loader_ 和 blp_loader_ 的作用是为以下类成员提供实例化：
        pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_;
        pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_;
        pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_;

        // set up plan triple buffer
        std::vector<geometry_msgs::PoseStamped> *planner_plan_;
        std::vector<geometry_msgs::PoseStamped> *latest_plan_;
        std::vector<geometry_msgs::PoseStamped> *controller_plan_;

        // set up the planner's thread
        bool runPlanner_;
        boost::recursive_mutex planner_mutex_;
        boost::condition_variable_any planner_cond_;        
        boost::thread *planner_thread_;
        boost::recursive_mutex configuration_mutex_;

        dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_;
        void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level);
        move_base::MoveBaseConfig last_config_;
        move_base::MoveBaseConfig default_config_;

        bool setup_, p_freq_change_, c_freq_change_;
        bool new_global_plan_;
    }
}