地图导航需要启动amcl节点和move_base节点，rviz节点。

1.amcl节点

(1)amcl接收从rviz中传送过来的初始化位置。

initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);

订阅rviz 发布的initialpose话题。如果收到rviz中传送过来的初始化位置调用AmclNode::initialPoseReceived函数。
（2）amcl向map_server发送信息，获取地图

requestMap()

（3）如果获取地图和初始化位置成功，初始化粒子滤波

AmclNode::applyInitialPose()

（4）初始化激光，广播tf信息。

 this->tfb_->sendTransform(tmp_tf_stamped);

2.move_base节点

（1）move_base节点从amcl接收tf消息，其中固定的frame为map，就是原点位置的名字叫做map，由于amcl中已经由粒子滤波计算出来了机器相对于地图的位置，用odom表示机器，那么amcl发送odom到map的tf在move_base节点中就可以接收到了。

tf::TransformListener tf(ros::Duration(10));
move_base::MoveBase move_base( tf );

（2）move_base节点新建局部地图和全局地图，并发送地图信息

 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);

其中Costmap2DROS类中发送地图信息

publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
                                      always_send_full_costmap);

（3）当move_base节点的局部地图和全局地图启动后，local_costmap从gazebo中接收里程计信息/odom和激光信息/scan并且更新局部地图。同时更新全局地图。

  // Start actively updating costmaps based on sensor data
    planner_costmap_ros_->start();

查看start函数发现将所有的插件设置为activate

(*plugin)->activate();

发现加载的插件为costmap_2d::InflationLayer ，costmap_2d::ObstacleLayer ，
costmap_2d::StaticLayer，costmap_2d::VoxelLayer
在costmap_2d::ObstacleLayer 类中的可以发现此类订阅了sensor_msgs::LaserScan消息

  boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan>
          > sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));

（4）move_base节点在executeCb函数中的 bool done = executeCycle(goal, global_plan)中发送cmd_vel信息，gazebo接收此信息并控制机器人运动，
首先新建一个路径规划类

 boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_
 planner_ = bgp_loader_.createInstance(global_planner);
 planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);

以上是使用插件的方式新建一个nav_core::BaseGlobalPlanne类。相当于new 一个这样的类。初始化的时候要将costmap地图传给该类。
初始化好后就要根据起始点，终点和地图来找全局路径了

planner_->makePlan(start, req.goal, global_plan)

找到路径后将路径发给局部地图规划器
nav_core::BaseLocalPlanne tc_
首先设置路径，

tc_->setPlan(*controller_plan_)

然后将根据路径计算控制命令

tc_->computeVelocityCommands(cmd_vel)

完成路径规划。

如何生成导航路径主要由makePlan来实现，具体使用什么算法生成导航路径将在下篇博文中介绍

下图为节点关系图
这里写图片描述

(七)turtlebot3导航包解析

1.amcl节点

2.move_base节点

猜你喜欢