参考文章
navigation总览
Navigation源码阅读之dwa_local_planner(DWA动态窗口法)
关于move_base的原理及应用
ROS navigation分析:navigation框架
0. nav_core
该包定义了整个导航系统关键包的接口函数,包括base_global_planner, base_local_planner以及recovery_behavior的接口。
里面的函数全是虚函数,所以该包只是起到规范接口的作用,真正功能的实现在相应的包当中。
1. move_base
move_base可以说是整个navigation代码阅读的入口,整个导航系统的初始化也是在这里进行,其主要功能如下
这个是整个navigation stack当中进行宏观调控的看得见的手。
它主要干的事情是这样的:
- 维护一张全局地图(基本上是不会更新的,一般是静态costmap类型),维护一张局部地图(实时更新,costmap类型);
- 维护一个全局路径规划器global_planner完成全局路径规划的任务, 维护一个局部路径规划器base_local_planner完成局部路径规划的任务。
- 然后提供一个对外的服务,负责监听nav_msgs::goal类型的消息,然后调动全局规划器规划全局路径,再将全局路径送进局部规划器,
- 局部规划器结合周围障碍信息(从其维护的costmap中查询),全局路径信息,目标点信息采样速度并评分获得最高得分的轨迹(即是采样的最佳速度),
- 然后返回速度值,由move_base发送Twist类型的cmd_vel消息上,从而控制机器人移动。完成导航任务。
move_base包运行后,我们仅需要在已经建立好的地图上指定目标位置和方向, move_base便会根据传感器(激光雷达,毫米波雷达等)获得的新的环境信息,以及自身的位置姿态,规划出一条全局的路径,然后局部路劲规划器根据机器人本身的状态和运动特性, 速度,加速度,位姿等规划出一系列的速度控制指令(并且进行速度模拟,碰撞检查).
白色底色方框内就是move_base的内容
必要的输入:
- goal : 期望机器人在地图中的目标位置
- tf : 各个坐标系之间的转换关系。(具体/map frame --> /odom frame ,/odom frame --> /base_link frame)
- odom:根据机器人左右轮速度推算出的航向信息(即/odom坐标系中机器人x,y坐标以及航向角yaw,下面会具体介绍
- LaserScan:激光传感器的信息,用于定位。(在这个系列教程中,我们没有用到这个激光信息,而是在一个假的空白地图上对机器人进行控制,并假定/map坐标系和/odom坐标系完全重合,在后面会有关于这两个坐标系的介绍)
可选的输入:
- amcl:自适应蒙特卡罗定位,一种用于2D环境下移动机器人的概率统计定位方法
- map:地图信息
输出:
- cmd_vel:在cmd_vel这个主题上发布Twist消息,这个消息包含的就是机器人的期望前进速度和转向速度。
move_base收到goal以后,将目标goal通过基于actionlib的client(客户端)向服务器发送,服务器根据tf关系以及发布的odom消息不断反馈机器人的状态(feedbackcall)到客户端, 让move_base做路径规划和控制twist。
1.1 MoveBase::MoveBase(tf::TransformListener& tf)
主要完成系统的默认参数的设置, 系统默认规划器的选择, 如全局规划器与局部规划器的选择(下面会介绍).当然,这是代码中的默认参数,也可以通过launch载入配置文件.xml进行修改.
<!-- The move_base node -->
<include file="$(find rbx1_nav)/launch/move_base.launch" />
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) {
//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);
1.2 MoveBase::executeCb(move_base_goal)
executeCb(move_base_goal)函数结构如下:
- executeCb(move_base_goal)函数收到运动的目标后,会首先转换到全局坐标系下;
然后开启MoveBase::planThread线程, 用于全局路径规划; - 开启costmap更新全局和局部costmap地图;
- 执行executeCycle() , 局部路径规划与控制部分
2. 全局路径规划
全局路径规划navigation中一共有三个实现,分别是global_planner, navfn,carrot_planner,都是实现当前点与目标点之间的路径规划, (在上面的MoveBase中可以看到默认使用的是navfn),其中global_planner, navfn内部都有Dijkstra算法和A*导航算法的实现.
planThread 中planner->makePlan(start, goal, plan)的全局路径规划代码逻辑见下图;
3. 局部路径规划
同样局部路劲规划也有两种实现:一是航迹推算法(TrajectoryROS),一是动态窗口法(DWA),都使用了nav_core::BaseLocalPlanner父类提供的接口
3.1 executeCycle()中实现了以下功能:
- getRobotPose //获取机器人当前位置
- publishFeedback as_发布位置状态反馈
- 是否走的足够远重置oscillation (避免震荡)
- if(new_global_plan_) 使用最新的全局路径
- move_base 状态机: move_base 在三种状态机中切换工作,PLANNING,CONTROLLING, CLEARING
3.2 tc_->computeVelocityCommands(cmd_vel) 计算局部控制速度并发布
- 1.将全局规划从规划帧转换到评价地图帧; transformGlobalPlan从全局路径中截取一定距离阈值内的路径作为transformed_plan
- 2.prunePlan 将之前的global路径点清除。(包括全局的global path和局部的global path)
- 3.robot_vel从发布的odom中获取速度信息
- 4.从全局plan中获取目标点坐标
- 5.与目标点距离是否小于阈值,或者已进入阈值内
- 6.findBestPath(global_pose, robot_vel, drive_cmds);
- 7.传递控制指令cmd_vel
- 8.发布局部和全局plan
4. costmap_2d
ROS基础教程–CostMap_2D包的一些理解这篇博客写的十分详细了,可以参考这篇博客.
costmap_2d主要是一个2D地图的创建和管理, costmap_2d::Layer下有主要的四个子类,表示地图中的四个子层,costmap初始化之后,便开始以update_frequency的频率更新地图,从外界传感器获得数据(如激光雷达,pointcloud),然后更新在地图的相应层上.