ROS导航-MoveBase

最新推荐文章于 2024-09-11 21:31:08 发布

慢慢的回味

最新推荐文章于 2024-09-11 21:31:08 发布

阅读量697

点赞数 2

分类专栏： ROS小车

本文链接：https://blog.csdn.net/bbxfnet/article/details/118239911

版权

MoveBase包通过全局规划器和局部规划器来实现当前地点到目标地点的导航。现在通过对其的源码解读来了解这个框架。http://liuxiaofei.com.cn/blog/

1 调用流程

--> Node创建：MoveBase::MoveBase(tf2_ros::Buffer& tf)
--> Action server（as_）接收导航目标：void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
--> runPlanner_值改变为true，导航线程void MoveBase::planThread()开始调用全局规划器planner_规划全局路径bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector& plan)
--> 一旦进入到控制状态CONTROLLING，MoveBase::executeCycle负责调用局部规划器tc_进行局部导航控制

2 Node创建

构造方法MoveBase::MoveBase：
初始化类加载器，全局规划器加载器bgp_loader_，局部规划器加载器blp_loader_，已经初始化一些全局变量。

  MoveBase::MoveBase(tf2_ros::Buffer& 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) {

创建一个Action Server，回调函数为executeCb：

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

获取node参数，默认全局规划器为NavfnROS，局部规划器为TrajectoryPlannerROS：

    ros::NodeHandle private_nh("~");
    ros::NodeHandle nh;

    recovery_trigger_ = PLANNING_R;

    //get some parameters that will be global to the move base node
    std::string global_planner, local_planner;
    private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
    private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
    private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
    private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
    private_nh.param("planner_frequency", planner_frequency_, 0.0);
    private_nh.param("controller_frequency", controller_frequency_, 20.0);
    private_nh.param("planner_patience", planner_patience_, 5.0);
    private_nh.param("controller_patience", controller_patience_, 15.0);
    private_nh.param("max_planning_retries", max_planning_retries_, -1);  // disabled by default

    private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
    private_nh.param("oscillation_distance", oscillation_distance_, 0.5);

设置3个plan，全局规划plan，最新的plan和局部控制plan，开启规划线程：

    //set up plan triple buffer
    planner_plan_ = new std::vector();
    latest_plan_ = new std::vector();
    controller_plan_ = new std::vector();

    //set up the planner's thread
    planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));

publish2个服务，cmd_vel发布速度，下位机收到后控制小车运动，action_goal_pub_发布当前目标到action server，goal_sub_订阅goal比如来自rviz里的，回调函数MoveBase::goalCB把goal封装成MoveBaseActionGoal发送给action server，而后引起回调MoveBase::executeCb：

    //for commanding the base
    vel_pub_ = nh.advertise("cmd_vel", 1);
    current_goal_pub_ = private_nh.advertise("current_goal", 0 );

    ros::NodeHandle action_nh("move_base");
    action_goal_pub_ = action_nh.advertise("goal", 1);

    //we'll provide a mechanism for some people to send goals as PoseStamped messages over a topic
    //they won't get any useful information back about its status, but this is useful for tools
    //like nav_view and rviz
    ros::NodeHandle simple_nh("move_base_simple");
    goal_sub_ = simple_nh.subscribe("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));

    //we'll assume the radius of the robot to be consistent with what's specified for the costmaps
    private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
    private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
    private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
    private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);

    private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
    private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
    private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);

    //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();

初始化全局规划器planner_ 和局部规划器tc_：