navigation总览
本文为对base_local_planner源码解读的前置研究。
navigation总览
- 1. navigation总览
- 2. move_base
- 2.1 move_base概况
- 2.2 move_base.launch分析
- 3.全局路径规划(暂时不涉及,留空)
- 4. 局部路径规划
1. navigation总览
打开一个终端,在github目录下输入
get clone https://github.com/ros-planning/navigation.git
下载navigation包,将会得到许多文件夹。其中主要的包作用如下:
nav_core
该包定义了整个导航系统关键包的接口函数,包括base_global_planner, base_local_planner以及recovery_behavior的接口。里面的函数全是虚函数,所以该包只是起到规范接口的作用,真正功能的实现在相应的包当中。之后添加自己编写的全局或局部导航算法也需要在这里声明。添加插件的总体描述以及教程链接
global_planner和navfn
这两个包干的事情是一样的,都是为实现目标点与当前点之间的全局路径规划,内部都有Dijkstra算法和A*导航算法的实现,ROS系统默认采用的是navfn。global_planner是新版并且已经稳定,仍然使用navfn是为了兼容性考虑,可以在nav_core中做修改。
base_local_planner
完成局部窗口内的路径规划任务,机器人行动速度的具体生成在此包当中完成。目前有两种局部路径规划算法实现,一是航迹推算法(TrajectoryROS),一是动态窗口法(DWA),该包内部的默认实现是航迹推算法,但是留出了DWA的定义接口,DWA的实现在dwa_local_planner中。
map_server
主要功能是读取pgm和yaml配套的地图文件,并将之转换到/map话题发送出来(比如rviz中显示的地图就是订阅了该话题), 另外还提供地图保存等增值服务。
costmap_2d
以层的概念来组织图层,用户可以根据需要自己配置,默认的层有static_layer(通过订阅map_server的/map主题)来生成, obstacle_layer根据传感器获得的反馈来生成障碍物图层, inflation_layer则是将前两个图层的信息综合进行缓冲区扩展。
该包可进行动态窗口的生成(比如base_local_planner中使用的就是动态的局部窗口地图)也可以生成静态地图(比如global_planner中使用的就是全局静态地图)。(有待验证的疑问,是否静态地图中obstacle_layer不起作用,而动态地图中static_layer不起作用)。
此外,该包还在实时发送自己的地图信息,通过nav_msgs::OccupancyGrid发送消息给rviz。所以可以仿照该写法来发送自己想要的数据到rviz中去。
rotate_recovery和clear_costmap_recovery
这两个包都继承自nav_core中定义的recovery_behavior类, 具体实现的是当导航发现无路可走的时候,机器人在原地打转转并清理(更新更恰当些)周围障碍物信息看是否有动态障碍物运动开,能找到路继续走。
在move_base当中,默认使用的recovery_behavior是这样的,先进行一次旋转,然后进行一次小半径的障碍物更新,然后再进行一次旋转,再进行一次大范围的障碍物更新,每进行一次recovery_behavior,都会重新尝试进行一次局部寻路,如果没找到,才会再执行下一个recovery_behavior。
这两个包的代码非常简单,不看也不会影响理解偏差,所以可以不用浪费时间看,当然因为简单要看也是分分钟的事。
move_base
这个是整个navigation stack当中进行宏观调控的看得见的手。它主要干的事情是这样的:
维护一张全局地图(基本上是不会更新的,一般是静态costmap类型),维护一张局部地图(实时更新,costmap类型),维护一个全局路径规划器global_planner完成全局路径规划的任务, 维护一个局部路径规划器base_local_planner完成局部路径规划的任务。
然后提供一个对外的服务,负责监听nav_msgs::goal类型的消息,然后调动全局规划器规划全局路径,再将全局路径送进局部规划器,局部规划器结合周围障碍信息(从其维护的costmap中查询),全局路径信息,目标点信息采样速度并评分获得最高得分的轨迹(即是采样的最佳速度),然后返回速度值,由move_base发送Twist类型的cmd_vel消息上,从而控制机器人移动,完成导航任务。
接下来会从move_base开始着手导航的整个过程。
2. move_base
2.1 move_base概况
ROS提供的move_base包让我们能够在已建立好的地图中指定目标位置和方向后,move_base根据机器人的传感器信息控制机器人到达我们想要的目标位置。它主要功能包括:结合机器人码盘推算出的odometry信息,作出路径规划,输出前进速度和转向速度。这两个速度是根据在配置文件里设定的最大速度和最小速度而自动作出的加减速决策。下面的白色底色方框内就是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。
知道了move_base的这些外围消息接口以后,move_base的运行还需要一些内部的配置参数,如机器人的最大最小速度,已经路径规划时的最大转弯半径等等,
2.2 move_base.launch分析
首先看的是move_base.launch中包括解析map,move_base和amcl定位三个部分,这构成了一个完整的框架 :
<launch> <param name="use_sim_time" value="false" /> <!-- EDIT THIS LINE TO REFLECT THE NAME OF YOUR OWN MAP FILE Can also be overridden on the command line --> <arg name="map" default="test_map.yaml" /> <!-- Run the map server with the desired map --> <node name="map_server" pkg="map_server" type="map_server" args="$(find dart_nav)/maps/dart.yaml"/> <!-- Start move_base --> <include file="$(find dart_nav)/launch/tb_move_base_test.launch" /> <!-- Fire up AMCL --> <include file="$(find dart_nav)/launch/tb_amcl.launch" /> </launch>
map以及amcl部分不是我们关心的重点,所以打开tb_move_base_test.launch,其配置如下:
<launch><node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true"> <rosparam file="$(find dart_nav)/config1/costmap_common_params.yaml" command="load" ns="global_costmap" /> <rosparam file="$(find dart_nav)/config1/costmap_common_params.yaml" command="load" ns="local_costmap" /> <rosparam file="$(find dart_nav)/config1/local_costmap_params.yaml" command="load" /> <rosparam file="$(find dart_nav)/config1/global_costmap_params.yaml" command="load" /> <rosparam file="$(find dart_nav)/config1/teb_local_planner_params.yaml" command="load" /><param name="base_global_planner" value="global_planner/GlobalPlanner"/> <param name="planner_frequency" value="1.0" /><param name="planner_patience" value="5.0" /> <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" /> <param name="controller_frequency" value="15.0" /> <param name="controller_patience" value="15.0" /> <rosparam file="$(find dart_nav)/config1/costmap_conversion_params.yaml" command="load" /> </node></launch>
如上所示,使用的是global_planner这个包,它默认使用的是dijkstra,当然也可以使用A*全局路径规划,局部路径规划使用的是teb,同样需要配置上面第3行到第7行的一些yaml,这些yaml是costmap和planner的一些配置文件。关于这些这些文件设置的作用,请看这里。可以使用rosrun rqt_reconfigure
命令实时对以上数据进行动态更改。
总体的设置这样就完成了,之后是对navigation包中的全局路径规划和局部路径规划的源代码进行分析。
3.全局路径规划(暂时不涉及,留空)
4. 局部路径规划
既然想要知道局部路径规划开始的过程,自然要从nav_core中开始,点开其中的base_loacl_planner.h文件,我们关心的信息如下:
//最为关键的地方,计算机器人下一刻的速度
virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
//判断是否到达目标点
virtual bool isGoalReached() = 0;
//加载全局路径
virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
//初始化
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
上文中关键部分为computeVelocityCommands,这个函数在trajectory_planner_ros.cpp,其主要实现框架如下所示:
bool TrajectoryPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { //检查初始化、检查是否已经到达目标点...略 transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan);//如果已经到达目标点,姿态还没到 if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) { tc_->updatePlan(transformed_plan); //所以这个函数里最关键的子函数是findBestPath Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds); return true; } tc_->updatePlan(transformed_plan); Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds); //然后又是转换,然后就发布出速度 }
下一步点开trajectory_planner.cpp,找到其中的子函数findBestPath,其主要实现框架如下:
Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel, tf::Stamped<tf::Pose>& drive_velocities) {//... Trajectory best = createTrajectories(pos[0], pos[1], pos[2], vel[0], vel[1], vel[2], acc_lim_x_, acc_lim_y_, acc_lim_theta_); //... }
最重要的函数是createTrajectories,是非常关键的函数,其源代码过长,直接给出主要实现框架:
Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta, double acc_x, double acc_y, double acc_theta) {//检查最终点是否是有效的,判断变量在updatePlan中被赋值if( final_goal_position_valid_ ) { double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y ); max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ ); } //是否使用dwa算法,sim_peroid_是1/controller_frequency_,暂时不清楚sim_period_和sim_time_的区别 if (dwa_) { max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_); min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_); max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_); min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_); } else { max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_); max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_); min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_); } //忽略其中的逻辑,按照不同的规则生成路径,调用的子函数是generateTrajectory
综上所述,整个base_loacl_planner的逻辑是这样的:
computeVelocityCommands->findBestTrajectory –> createTrajectories –> generateTrajectory
最终,选择分数最低的轨迹,发布出去。这便是整个局部规划器的实现思路和逻辑。
navigation总览相关推荐
- RNN, LSTM, GRU模型的作用, 构建, 优劣势比较,attention机制
查看全文 http://www.taodudu.cc/news/show-5952629.html 相关文章: RNN基本原理及代码实战 高通camera hal3学习 高通平台sensor框架图[学 ...
- Android Jetpack组件之Navigation使用-源码
1.前言 最近简单看了下google推出的框架Jetpack,感觉此框架的内容可以对平时的开发有很大的帮助,也可以解决很多开发中的问题,对代码的逻辑和UI界面实现深层解耦,打造数据驱动型UI界面. A ...
- Android Jetpack组件总览
1.前言 最近简单看了下google推出的框架Jetpack,感觉此框架的内容可以对平时的开发有很大的帮助,也可以解决很多开发中的问题,对代码的逻辑和UI界面实现深层解耦,打造数据驱动型UI界面. A ...
- Android Jetpack总览
Android Jetpack总览 Jetpack 是一套库.工具和指南,可帮助开发者更轻松地编写优质应用.这些组件可帮助您遵循最佳做法.让您摆脱编写样板代码的工作并简化复杂任务,以便您将精力集中放在 ...
- android上方导航条跳转页面,《成为大前端》系列 7. 多页面、页面跳转和Navigation模块...
介绍 开发过移动 Web 页面的同学都知道,单个页面由客户端的 UI 所承载,页面间的跳转也 不再是使用 window 和 location,也不是使用 a 标签,而且调用 Native 写好的 br ...
- getinstance方法详解_二、设计模式总览及工厂模式详解
二.架构师内功心法之设计模式 2.架构师内功心法之设计模式 2.1.课程目标 1.通过对本章内容的学习,了解设计模式的由来. 2.介绍设计模式能帮我们解决哪些问题. 3.剖析工厂模式的历史由来及应用场 ...
- 三维点云语义分割总览
点云PCL免费知识星球,点云论文速读. 标题:三维点云语义分割总览 作者:吉祥街 欢迎各位加入免费知识星球,获取PDF文档,欢迎转发朋友圈,分享快乐. 希望有更多的小伙伴能够加入我们,一起开启论文阅读 ...
- 程序员新手 0年份等级 指导(一) 开发人员IT架构总览
程序员新手 0年份等级 指导(一) 开发人员IT架构总览 程序员新手 0年份等级 指导(一) 开发人员相关IT架构总览之职能分解 开发人员IT架构总览 一.职能分解 软件项目的主要组成大体上按照一个项 ...
- iPhone App开发导航条(Navigation Bar)素材PSD下载
不管是iPhone还是Android的应用App界面基本上最上方都会有个导航条(Navigation Bar).于是我决定创建此页面整理收集所有好看的适合在iPhone App应用开发中使用的导航条素 ...
最新文章
- 您如何与Docker的流程连接和分离?
- php 正则表达式 取所有内容,php正则表达式获取内容所有链接
- 数据结构(一)---顺序表的实现---java版
- 范数在机器学习中的作用_设计在机器学习中的作用
- 畅通工程续 最短路
- RK3308(2) --- 上手教程
- 手持“六脉神剑”、横跨软硬领域,揭晓英特尔构筑智慧云基石宝典!
- L2-038 病毒溯源 (25 分)-PAT 团体程序设计天梯赛 GPLT
- BestCoder Round #89
- 中国联通骨干网网络介绍
- UDS(ISO14229)诊断服务功能及描述完结篇
- 超级好用的一个php上传图片类(随机名_缩略图_加水印),php教程_超级好用的一个php上传图片类(随机名,缩略图,加水印)...
- 网页倒计时跳转JS代码
- 前锋java退学_曾经的第一高中生为打CBA从清华大学退学,本赛季场均仅3.1分!...
- pc android 凤凰,应用多开,这才是最适用电脑的安卓—凤凰系统2.0
- WINVNC(二)omni_thread
- 官方正版授权Apowersoft 傲软抠图AI智能换背景工具软件
- 【springboot】5、lombok
- 学习c/c++ 推荐学习什么书籍?
- EtherCAT学习笔记:EEPROM存储内容结构(从站配置信息接口SII)