ROS navigation: move_base笔记：代码详解

• 文件内容

move_base: 1.14.4,源码下载地址：https://github.com/ros-planning/navigation.git

• move_base_node.cpp文件

#include 

int main(int argc, char** argv){
  ros::init(argc, argv, "move_base_node");
  tf::TransformListener tf(ros::Duration(10));//接收tf数据，监听

  move_base::MoveBase move_base( tf );//用户调用movebase通过传入带有tf参数的构造函数

  //ros::MultiThreadedSpinner s;
  ros::spin();

  return(0);
}

ros::Duration（10）#include表示某个时间段

tf::TransformListener

构造函数

TransformListener::TransformListener(
ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME),
bool spin_thread = true	 
)

DEFAULT_CACHE_TIME：存储转换信息时间，由于tf树会随着时间而变化

 

• move_base.cpp

参考

http://wiki.ros.org/move_base/

https://www.cnblogs.com/flyinggod/p/9081194.html

http://docs.ros.org/diamondback/api/move_base/html/classmove__base_1_1MoveBase.html#_details

• 构造函数的初始化列表

MoveBase::MoveBase(tf::TransformListener& tf) :
    tf_(tf),
    as_(NULL),
    planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
    bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
    blp_loader_("nav_core", "nav_core::BaseLocalPlanner"), 
    recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
    planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
    runPlanner_(false), setup_(false), p_freq_change_(false),         
    c_freq_change_(false), new_global_plan_(false)

参数列表解释：在move_base.h中定义

1. boost::shared_ptr tc_;

   boost::shared_ptr shared_ptr行为与寻常指针一致，所以平时我们在编写C++代码时，可以用shared_ptr去替代普通指针 nav_core::BaseLocalPlanner

   #include 为导航中使用的本地计划者提供一个接口

2. MoveBaseActionServer* as_;
   
   
   typedef actionlib::SimpleActionServer move_base::MoveBaseActionServer
   

   参考：http://docs.ros.org/diamondback/api/move_base/html/namespacemove__base.html
   
   move_base_msgs::MoveBaseAction Message

   MoveBaseActionGoal action_goal
       Header header
               ＃用于高级标记数据类型的标准元数据。
               ＃通常用于在特定坐标系中传递带时间戳的数据  。
               ＃ 序列ID：连续增加的ID 
           uint32 seq
               ＃两个整数时间戳记，表示为：
               ＃* stamp.secs：自历元以来的秒数（stamp_secs）
               ＃* stamp.nsecs：自stamp_secs以来的纳秒数
               ＃时间处理糖由客户端库
           time stamp
               ＃与该数据关联的帧
               ＃0：无帧＃1：全局帧
           string frame_id
       actionlib_msgs/GoalID goal_id
               ＃时间戳应存储要求达到此目标的时间。
               ＃当动作服务器尝试抢占所有在某个时间戳之前请求的目标时，它将被使用。
           time stamp
               ＃ID提供了一种将反馈和结果消息与特定目标请求相关联的方法。指定的ID必须是唯一的。
           string ID
       MoveBaseGoal goal
           geometry_msgs/PoseStamped target_pose
   
   MoveBaseActionResult action_result
       Header header #同上
       actionlib_msgs/GoalStatus status
           GoalID goal_ID         #同上
           uint8 status
           uint8 PENDING= 0        ＃尚未由动作服务器处理目标
           uint8 ACTIVE = 1       ＃目标当前正在由动作服务器处理
           uint8 PREEMPTED = 2    ＃目标开始执行后收到取消请求并已完成执行（终端状态）
           uint8 SUCCEEDED = 3    ＃目标已由操作服务器成功完成（终端状态）
           uint8 ABORTED = 4      ＃目标在操作服务器执行过程中由于失败而中止（终端状态）
           uint8 REJECTED= 5      ＃目标被操作服务器拒绝而未处理，因为目标无法实现或无效（终端状态）
           uint8 PREEMPTING = 6   ＃目标在开始执行后收到取消请求，尚未完成执行
           uint8 RECALLING = 7    ＃目标在开始执行之前已收到取消请求，但动作服务器尚未确认目标已被取消
           uint8 RECALLED = 8     ＃目标在开始执行之前已收到取消请求，并已成功取消（终端状态）
           uint8 LOST = 9         ＃一个动作客户端可以确定目标是LOST。这不应该是由动作服务器通过电线发送的
   
           #允许用户将字符串与GoalStatus关联以调试
           string text
       MoveBaseResult result
   
   MoveBaseActionFeedback action_feedback
       Header header    #同上
       actionlib_msgs/GoalStatus status #同上
       MoveBaseFeedback feedback
           geometry_msgs/PoseStamped base_position

   actionlib :: SimpleActionServer 单一目标策略。该策略的规范如下：一次只能有一个目标处于活动状态，新目标会根据其GoalID字段中的标记来抢占先前的目标（后继的目标抢占较早的目标），明确的先占目标会抢占具有如果时间戳小于或等于与抢占相关的时间戳，则接受新目标意味着成功抢占了所有旧目标，并且旧目标的状态将自动更改以反映这一情况。

3. costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;

   costmap_2d::Costmap2DROS 2D Costmap的ROS包装器。处理订阅主题的主题，这些主题以PointCloud或LaserScan消息的形式提供有关障碍的观察。

4.  pluginlib::ClassLoader bgp_loader_;//模版类
    pluginlib::ClassLoader blp_loader_;
    pluginlib::ClassLoader recovery_loader_;
   
    bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"),
    blp_loader_("nav_core", "nav_core::BaseLocalPlanner"), 
    recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),

    pluginlib::ClassLoader

   #include 有助于管理和加载类的类；“nav_core”包含基类的包；“nav_core::BaseGlobalPlanner”要加载的基类类型：
   nav_core::BaseGlobalPlanner
   为导航中使用的全球计划者提供一个接口。所有编写为导航堆栈插件的全局计划程序都必须遵守此接口
   nav_core::BaseLocalPlanner
   为导航中使用的本地计划者提供一个接口。所有编写为导航堆栈插件的本地计划人员都必须遵守此接口。
   nav_core::RecoveryBehavior 
   提供用于导航中的恢复行为的接口。所有写为导航堆栈插件的恢复行为都必须遵守此接口。

5.  std::vector* planner_plan_;
    std::vector* latest_plan_;
    std::vector* controller_plan_;

    

6. bool runPlanner_;//规划器是否启用标志位
   bool setup_, p_freq_change_, c_freq_change_;
   bool new_global_plan_;

    

• 进入构造函数

as_=new MoveBaseActionServer(ros::NodeHandle(),"move_base",boost::bind(&MoveBase::executeCb,this,_1),false);

在actionlib :: SimpleActionServer的构造函数中

actionlib::SimpleActionServer< ActionSpec >::SimpleActionServer	(	
ros::NodeHandle    n,                //A NodeHandle to create a namespace under
std::string        name,             //A name for the action server
ExecuteCallback    execute_cb,       //可选回调，当收到新目标时在单独线程中调用，允许用户进行阻塞回调。添加一个execute回调也会停用goalCallback。
bool               auto_start)	    //一个布尔值，它通知ActionServer是否在发布后立即开始发布。应将此设置为FALSE以避免冲突条件，并且应在构造服务器后调用start（）

其中

typedef boost::function  actionlib::SimpleActionServer ::ExecuteCallback

boost::bind(&MoveBase::executeCb,this,_1)

boost::bind，_1占位符

actionServer状态机，并且新建了一个executeCb线程 

MoveBase::executeCb分析：这个线程主要用于接收目标点，并进行坐标变换，并转化为成员变量，将其用话题的方式再发布出去。

• 接下来从private_nh中获取参数设定值

ros::NodeHandle private_nh("~");//私有节点ros::NodeHandle pn1("~"); //pn1 命名空间为/node_namespace/node_name

ros::NodeHandle nh;

recovery_trigger_ = PLANNING_R;

1. ROS::NodeHandle 官网2

2.   RecoveryTrigger recovery_trigger_
     enum RecoveryTrigger
     {
       PLANNING_R,
       CONTROLLING_R,
       OSCILLATION_R
     };

3. ros::nodehandle::param(const std::string & param_name,T & param_val,const T & default_val )
   此方法尝试从参数服务器检索指示的参数值，并将结果存储在param_val中。 如果无法从服务器检索该值，则使用default_val。
   param_name:参数服务器中的参数名
   param_val：将该参数名的值读取到param_val。
   default_val：指定默认的值。
   

• 设置plan buffer

    //set up plan triple buffer用于路劲规划的三重缓冲区 全局规划
    planner_plan_ = new std::vector();
    latest_plan_ = new std::vector();
    controller_plan_ = new std::vector();

• 开启planner thread

    //set up the planner's thread
    planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));//创建一个新的线程用于路径规划

boost::thread* planner_thread_; 

MoveBase::planThread分析

• 创建publisher----->cmd_vel,current_goal,goal;订阅goal，回调函数是MoveBase::goalCB分析:

    void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
      ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
      move_base_msgs::MoveBaseActionGoal action_goal;
      action_goal.header.stamp = ros::Time::now();
      action_goal.goal.target_pose = *goal;
  
      action_goal_pub_.publish(action_goal);
    }

  订阅命名空间“move_base_simple”下的goal在命名空间“move_base”下进行发布

• 接下来设置机器人的几何尺寸

• 实例化全局和局部代价地图

      //create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
      planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
      planner_costmap_ros_->pause();
  
      //initialize the global planner实例化
      try {
        planner_ = bgp_loader_.createInstance(global_planner);
        planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
      } catch (const pluginlib::PluginlibException& ex) {
        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());
        exit(1);
      }
  
      //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
      controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
      controller_costmap_ros_->pause();
  
      //create a local planner
      try {
        tc_ = blp_loader_.createInstance(local_planner);
        ROS_INFO("Created local_planner %s", local_planner.c_str());
        tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
      } catch (const pluginlib::PluginlibException& ex) {
        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());
        exit(1);
      }

   

1. costmap_2d::Costmap2DROS 存储和更新有关世界上障碍物的信息，官网解释

   costmap_2d::Costmap2DROS::Costmap2DROS	(	
   std::string 	name, //代价地图的名称
   tf::TransformListener & 	tf 
   )		
   

    

2. boost::shared_ptr planner_;

   planner_ = bgp_loader_.createInstance(global_planner);要加载类的名称“global_planner”，在参数服务器中的参数名为“base_global_planner”；planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);初始化

3. // Start actively updating costmaps based on sensor data开启costmap基于传感器数据的更新
       planner_costmap_ros_->start();
       controller_costmap_ros_->start();

   订阅传感器主题并启动costmap更新

4. ros::ServiceServer make_plan_srv_, clear_costmaps_srv_;
   
   make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
   clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

   创建服务器，消息到达时调用的成员函数：

• MoveBase::planServicereq参数包含了起点和目标信息。首先判断global planner 的costmap是否存在以及是否能够得到机器人所在位置，然后调用clearCostmapWindows 完成对机器人区域的clear，然后调用 if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){} 这是完成plan计算的核心部分。逻辑判断这个调用是否成功，如果失败，则在目标区域附件搜索，更改req.goal的值，并重新调用makePlan，如果失败，则此次plan失败，无解。
• loadRecoveryBehaviors      loadDefaultRecoveryBehaviors();加载一些恢复行为
• 开启action，开启参数动态配置服务