移动机器人——ros navigation
一、概述
ROS navigation为移动机器人导航相关包的集合，实现定位规划避障等相关功能。
整体工作流程为：
-
加载地图
navigation通过map_server加载现有地图。navigation无建图相关包，需另外实现后保存，默认只支持2维地图，其他包类似grid_map提供2.5维地图的加载。
-
发布TF坐标变换
navigation包内的AMCL提供map->odom的变换，AMCL实际为全局绝对定位，即map->base，但在应用时里程计会提供odom->base的变换，因此AMCL会获取TF信息转换后再发布map->base变换，navigation不包含里程计的实现，需另外独立实现。
-
启动move_base加载相关插件
move_base为导航的核心框架，通过预设接口（plugin）加载地图（costmap_2d）和规划避障（global_planner, local_planner, recovery）相关插件，读取设置参数（机器人外形，规划避障设置等），并发布相关话题和服务器。
总得来说要实现机器人的自主导航除navigation外还需要一张图和里程计，当然navigation内部的算法都可以根据需要进行更换。
在实际使用后发现，move_base因其设计问题，灵活度较低，只能满足室内基本功能的实现，改进起来也不太方便，若需在复杂场景下使用可参考move_base_flex或者更换navigation2。
二、move_base
基础框架
工作流程：
-
通过topic/Action设置目标点。
-
调用global_planner规划全局路径。
-
根据全局路径调用local_planner计算优化路径。
-
执行优化路径，根据设置参数重复调用2，3步，直至到底终点。
ps：global_planner按照启动时设置的参数按照频率重复调用，无法中途修改，local_planner移动一段距离后重复调用
-
没有有效路径时，调用recovery_behaviors进行恢复，然后重规划，若成功回到步骤4，失败则按顺序执行设定的行为，全部失败则该运动失败。
Action
Action Subscribed Topics
-
move_base/goal(move_base_msgs/MoveBaseActionGoal)A goal for
move_baseto pursue in the world. -
move_base/cancel(actionlib_msgs/GoalID)A request to cancel a specific goal.
Action Published Topics
-
move_base/feedback(move_base_msgs/MoveBaseActionFeedback)Feedback contains the current position of the base in the world.
-
move_base/status(actionlib_msgs/GoalStatusArray)Provides status information on the goals that are sent to the
move_baseaction. -
move_base/result(move_base_msgs/MoveBaseActionResult)Result is empty for the
move_baseaction.
参数配置
-
base_global_planner(string, default:"navfn/NavfnROS"For 1.1+ series)The name of the plugin for the global planner to use with
move_base, see pluginlib documentation for more details on plugins. This plugin must adhere to thenav_core::BaseGlobalPlannerinterface specified in the nav_core package. (1.0 series default:"NavfnROS") -
base_local_planner(string, default:"base_local_planner/TrajectoryPlannerROS"For 1.1+ series)The name of the plugin for the local planner to use with
move_basesee pluginlib documentation for more details on plugins. This plugin must adhere to thenav_core::BaseLocalPlannerinterface specified in the nav_core package. (1.0 series default:"TrajectoryPlannerROS") -
recovery_behaviors(list, default: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}] For 1.1+ series)A list of recovery behavior plugins to use with
move_base, see pluginlib documentation for more details on plugins. These behaviors will be run whenmove_basefails to find a valid plan in the order that they are specified. After each behavior completes,move_basewill attempt to make a plan. If planning is successful,move_basewill continue normal operation. Otherwise, the next recovery behavior in the list will be executed. These plugins must adhere to thenav_core::RecoveryBehaviorinterface specified in the nav_core package.-
name: conservative_reset, type: ClearCostmapRecovery
-
name: rotate_recovery, type: RotateRecovery
-
name: aggressive_reset, type: ClearCostmapRecovery
Note: For the default parameters, the aggressive_reset behavior will clear out to a distance of 4 *
~/local_costmap/circumscribed_radius.
-
-
controller_frequency(double, default: 20.0)The rate in Hz at which to run the control loop and send velocity commands to the base. 控制频率会影响小车移动速度和控制效果，teb算法中dt_ref与该参数有关，同时设置频率不一定为能达到的频率，无法达到则会报警告，然后重新调整该参数。
-
planner_patience(double, default: 5.0)
How long the planner will wait in seconds in an attempt to find a valid plan before space-clearing operations are performed. -
controller_patience(double, default: 15.0)How long the controller will wait in seconds without receiving a valid control before space-clearing operations are performed.
-
conservative_reset_dist(double, default: 3.0)The distance away from the robot in meters beyond which obstacles will be cleared from the costmap when attempting to clear space in the map. Note, this parameter is only used when the default recovery behaviors are used for
move_base. -
recovery_behavior_enabled(bool, default:true)Whether or not to enable the
move_baserecovery behaviors to attempt to clear out space. -
clearing_rotation_allowed(bool, default:true)Determines whether or not the robot will attempt an in-place rotation when attempting to clear out space. Note: This parameter is only used when the default recovery behaviors are in use, meaning the user has not set the
recovery_behaviorsparameter to anything custom. -
shutdown_costmaps(bool, default:false)Determines whether or not to shutdown the costmaps of the node when
move_baseis in an inactive state -
oscillation_timeout(double, default: 0.0)How long in seconds to allow for oscillation before executing recovery behaviors. A value of 0.0 corresponds to an infinite timeout. New in navigation 1.3.1
-
oscillation_distance(double, default: 0.5)How far in meters the robot must move to be considered not to be oscillating. Moving this far resets the timer counting up to the
~oscillation_timeoutNew in navigation 1.3.1 -
planner_frequency(double, default: 0.0)The rate in Hz at which to run the global planning loop. If the frequency is set to 0.0, the global planner will only run when a new goal is received or the local planner reports that its path is blocked. New in navigation 1.6.0
-
max_planning_retries(int32_t, default: -1)How many times to allow for planning retries before executing recovery behaviors. A value of -1.0 corresponds to an infinite retries.
基础应用
move_base提供action和topic两种形式的调用
设置action
//load MoveBaseAction，and wait for the action server to come up
MoveBaseClient ac("move_base", true);
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");}
定点运动
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.header.frame_id = "map";//global goal
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = 1;
goal.target_pose.pose.position.y = 1;
goal.target_pose.pose.position.z = 0.0;
goal.target_pose.pose.orientation.x = 0.0;
goal.target_pose.pose.orientation.y = 0.0;
goal.target_pose.pose.orientation.z = 0.0;
goal.target_pose.pose.orientation.w = 1.0;
ROS_INFO("Sending goal");
ac.sendGoal(goal);
ac.waitForResult();
取消运动
ac.cancelAllGoals();
ac.waitForResult();
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("succeeded");
else
ROS_INFO("fall");
三、move_base_flex
Move Base Flex (MBF), a backwards-compatible replacement for move_base. MBF can use existing plugins for move_base, and provides an enhanced version of the same ROS interface. It exposes action servers for planning, controlling and recovering, providing detailed information of the current state and the plugin’s feedback. An external executive logic can use MBF and its actions to perform smart and flexible navigation strategies.
move_base_flex是对move_base的加强，使用和move_base相同的plugin接口的同时拓展了接口virtual函数，保留了move_base相关的topic和servers，同时提供各独立功能的action和状态检测相关的servers，可使用SMACH或者BehaviourTrees灵活组合调用各功能。
基于状态机实现的move_base原始的任务决策
Action
-
MoveBase
# Extension of move_base_msgs/MoveBase action, with more detailed result # and feedback and the possibility to specify lists of applicable plugins geometry_msgs/PoseStamped target_pose # Controller to use; defaults to the first one specified on "controllers" parameter string controller # Planner to use; defaults to the first one specified on "planners" parameter string planner # Recovery behaviors to try on case of failure; defaults to the "recovery_behaviors" parameter value string[] recovery_behaviors --- # Predefined success codes: uint8 SUCCESS = 0 # Predefined general error codes: uint8 FAILURE = 10 uint8 CANCELED = 11 uint8 COLLISION = 12 uint8 OSCILLATION = 13 uint8 START_BLOCKED = 14 uint8 GOAL_BLOCKED = 15 uint8 TF_ERROR = 16 uint8 INTERNAL_ERROR = 17 # 21..49 are reserved for future general error codes # Planning/controlling failures: uint8 PLAN_FAILURE = 50 # 51..99 are reserved as planner specific errors uint8 CTRL_FAILURE = 100 # 101..149 are reserved as controller specific errors uint32 outcome string message # Configuration upon action completion float32 dist_to_goal float32 angle_to_goal geometry_msgs/PoseStamped final_pose --- # Outcome of most recent controller cycle. Same values as in MoveBase or ExePath result. uint32 outcome string message float32 dist_to_goal float32 angle_to_goal geometry_msgs/PoseStamped current_pose geometry_msgs/TwistStamped last_cmd_vel # last command calculated by the controller -
ExePath
# Follow the given path until completion or failure nav_msgs/Path path # Controller to use; defaults to the first one specified on "controllers" parameter string controller # use different slots for concurrency uint8 concurrency_slot # define goal tolerance for the action bool tolerance_from_action float32 dist_tolerance float32 angle_tolerance --- # Predefined success codes: uint8 SUCCESS = 0 # 1..9 are reserved as plugin specific non-error results # Predefined error codes: uint8 FAILURE = 100 # Unspecified failure, only used for old, non-mfb_core based plugins uint8 CANCELED = 101 uint8 NO_VALID_CMD = 102 uint8 PAT_EXCEEDED = 103 uint8 COLLISION = 104 uint8 OSCILLATION = 105 uint8 ROBOT_STUCK = 106 uint8 MISSED_GOAL = 107 uint8 MISSED_PATH = 108 uint8 BLOCKED_PATH = 109 uint8 INVALID_PATH = 110 uint8 TF_ERROR = 111 uint8 NOT_INITIALIZED = 112 uint8 INVALID_PLUGIN = 113 uint8 INTERNAL_ERROR = 114 uint8 OUT_OF_MAP = 115 # The start and / or the goal are outside the map uint8 MAP_ERROR = 116 # The map is not running properly uint8 STOPPED = 117 # The controller execution has been stopped rigorously. # 121..149 are reserved as plugin specific errors uint32 outcome string message geometry_msgs/PoseStamped final_pose float32 dist_to_goal float32 angle_to_goal --- # Outcome of most recent controller cycle. Same values as in result uint32 outcome string message float32 dist_to_goal float32 angle_to_goal geometry_msgs/PoseStamped current_pose geometry_msgs/TwistStamped last_cmd_vel # last command calculated by the controller -
GetPath
# Get a path from start_pose or current position to the target pose # Use start_pose or current position as the beginning of the path bool use_start_pose # The start pose for the path; optional, used if use_start_pose is true geometry_msgs/PoseStamped start_pose # The pose to achieve with the path geometry_msgs/PoseStamped target_pose # If the goal is obstructed, how many meters the planner can relax the constraint in x and y before failing float64 tolerance # Planner to use; defaults to the first one specified on "planners" parameter string planner # use different slots for concurrency uint8 concurrency_slot --- # Predefined success codes: uint8 SUCCESS = 0 # 1..9 are reserved as plugin specific non-error results # Possible error codes: uint8 FAILURE = 50 # Unspecified failure, only used for old, non-mfb_core based plugins uint8 CANCELED = 51 # The action has been canceled by a action client uint8 INVALID_START = 52 # uint8 INVALID_GOAL = 53 uint8 NO_PATH_FOUND = 54 uint8 PAT_EXCEEDED = 55 uint8 EMPTY_PATH = 56 uint8 TF_ERROR = 57 uint8 NOT_INITIALIZED = 58 uint8 INVALID_PLUGIN = 59 uint8 INTERNAL_ERROR = 60 uint8 OUT_OF_MAP = 61 uint8 MAP_ERROR = 62 uint8 STOPPED = 63 # The planner execution has been stopped rigorously. # 71..99 are reserved as plugin specific errors uint32 outcome string message nav_msgs/Path path float64 cost --- -
Recovery
# Run one of the recovery behavior listed on recovery_behaviors parameter string behavior # use different slots for concurrency uint8 concurrency_slot --- # Predefined success codes: uint8 SUCCESS = 0 # Possible server codes: uint8 FAILURE = 150 uint8 CANCELED = 151 uint8 PAT_EXCEEDED = 152 uint8 TF_ERROR = 153 uint8 NOT_INITIALIZED = 154 uint8 INVALID_PLUGIN = 155 uint8 INTERNAL_ERROR = 156 uint8 STOPPED = 157 # The recovery behaviour execution has been stopped rigorously. uint8 IMPASSABLE = 158 # Further execution would lead to a collision # 171..199 are reserved as plugin specific errors uint32 outcome string message string used_plugin ---
servers
-
CheckPath
uint8 LOCAL_COSTMAP = 1 uint8 GLOBAL_COSTMAP = 2 nav_msgs/Path path # the path to be checked after transforming to costmap frame float32 safety_dist # minimum distance allowed to the closest obstacle (footprint padding) float32 lethal_cost_mult # cost multiplier for cells marked as lethal obstacle (zero is ignored) float32 inscrib_cost_mult # cost multiplier for cells marked as inscribed obstacle (zero is ignored) float32 unknown_cost_mult # cost multiplier for cells marked as unknown space (zero is ignored) uint8 costmap # costmap in which to check the pose uint8 return_on # abort check on finding a pose with this state or worse (zero is ignored) uint8 skip_poses # skip this number of poses between checks, to speedup processing bool path_cells_only # check only cells directly traversed by the path, ignoring robot footprint # (if true, safety_dist is ignored) --- uint8 FREE = 0 # robot is completely in traversable space uint8 INSCRIBED = 1 # robot is partially in inscribed space uint8 LETHAL = 2 # robot is partially in collision uint8 UNKNOWN = 3 # robot is partially in unknown space uint8 OUTSIDE = 4 # robot is completely outside the map uint32 last_checked # index of the first pose along the path with return_on state or worse uint8 state # path worst state (until last_checked): FREE, INFLATED, LETHAL, UNKNOWN... uint32 cost # cost of all cells traversed by path within footprint padded by safety_dist # or just by the path if path_cells_only is true -
CheckPoint
uint8 LOCAL_COSTMAP = 1 uint8 GLOBAL_COSTMAP = 2 geometry_msgs/PointStamped point # the point to be checked after transforming to costmap frame uint8 costmap # costmap in which to check the point --- uint8 FREE = 0 # point is in traversable space uint8 INSCRIBED = 1 # point is in inscribed space uint8 LETHAL = 2 # point is in collision uint8 UNKNOWN = 3 # point is in unknown space uint8 OUTSIDE = 4 # point is outside the map uint8 state # point state: FREE, INFLATED, LETHAL, UNKNOWN or OUTSIDE uint32 cost # cost of the cell at point -
CheckPose
uint8 LOCAL_COSTMAP = 1 uint8 GLOBAL_COSTMAP = 2 geometry_msgs/PoseStamped pose # the pose to be checked after transforming to costmap frame float32 safety_dist # minimum distance allowed to the closest obstacle float32 lethal_cost_mult # cost multiplier for cells marked as lethal obstacle (zero is ignored) float32 inscrib_cost_mult # cost multiplier for cells marked as inscribed obstacle (zero is ignored) float32 unknown_cost_mult # cost multiplier for cells marked as unknown space (zero is ignored) uint8 costmap # costmap in which to check the pose bool current_pose # check current robot pose instead (ignores pose field) --- uint8 FREE = 0 # robot is completely in traversable space uint8 INSCRIBED = 1 # robot is partially in inscribed space uint8 LETHAL = 2 # robot is partially in collision uint8 UNKNOWN = 3 # robot is partially in unknown space uint8 OUTSIDE = 4 # robot is completely outside the map uint8 state # pose state: FREE, INFLATED, LETHAL, UNKNOWN or OUTSIDE uint32 cost # total cost of all cells within footprint padded by safety_dist
四、map_server
用于加载和保存二维地图信息。
地图格式
地图包含两部分，地图文件和地图描述yaml
Image format
通过SDL_Image读取，支持多种格式，map_server保存默认生成pgm（P5，灰度图，二进制）格式。
YAML format
image: testmap.pgm
resolution: 0.1
origin: [0.0, 0.0, 0.0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
- image : 照片路径，绝对路径或者YAML文件所在的相对路径
- resolution : 分辨率，米/像素
- origin ：图片最左下角像素在地图中的2d位置 ( x , y , y a w ) (x,y,yaw) (x,y,yaw)，yaw为逆时针偏转，当yaw=0时没有偏转，许多系统会忽略yaw。
- occupied_thresh：大于这个阀值的占用概率的像素被认为occupied
- free_thresh：小于这个阀值的占用概率的像素被认为是free
- negate ：是否对white/black free/occupied定于的语义进行翻转，对occupied_thresh/free_thresh无影响。
- mode(Optional)：有三种模式可选trinary, scale, raw，默认trinary
p为占用概率，x为像素值（白255，黑0）
-
negate为false时， p = 255 − x 255 p=\frac{255-x}{255} p=255255−x
-
negate为false时， p = x 255 p=\frac{x}{255} p=255x
在Trinary模式下：
- If
p > occupied_thresh, output the value100to indicate the cell is occupied. - If
p < free_thresh, output the value0to indicate the cell is free. - Otherwise, output
-1a.k.a.255(as an unsigned char), to indicate that the cell is unknown.
命令行工具
map_server 加载地图
rosrun map_server map_server mymap.yaml
-
Published Topics
map_metadata(nav_msgs/MapMetaData)- Receive the map metadata via this latched topic.
map(nav_msgs/OccupancyGrid)- Receive the map via this latched topic.
-
Services
static_map(nav_msgs/GetMap)- Retrieve the map via this service.
-
Parameters
~frame_id(string, default:"map")- The frame to set in the header of the published map.
map_saver 保存地图
rosrun map_server map_saver [--occ <threshold_occupied>] [--free <threshold_free>] [-f <mapname>] map:=/your/costmap/topic
- mapname：地图储存的路径和名字
- map：地图话题，默认为
map
使用
rosrun map_server map_saver -f mymap
五、Planner
global_planner
-
navfn 默认使用，但其A*算法存在BUG
-
global_planner 修正了BUG，为navfn替代版
使用（安装
ros-melodic-global-planner）：> -
nav2_smac_planner navigation2的新的规划算法
local_planner
- dwa_local_planner 默认规划器
- teb_local_planner 测试效果最好，但参数较多，配置复杂
六、recovery
move_base节点可以在机器人认为自己被卡住时选择性地执行恢复行为。默认情况下， move_base 节点将采取以下操作来清除空间：
地图基于特定策略计算概率进行更新，参考《概率机器人》，地图部分只有在获得数据的情况下才会更新，因此某个区域识别到障碍物后，即使物体已经移动，若不重新扫描，该区域依然存在障碍物，同时因策略的不同，在识别到障碍物后的更新速度和效果有可能会与理想状态不一致。
static即现有地图障碍物不会改变，上述全为非现有地图障碍物。
- 从地图上清除用户指定区域以外的障碍物
- 原地旋转清除周围障碍物
- 清除矩形区域之外的所有障碍（在这个区域内机器人可以原地旋转）
- 再一次原地旋转清除周围障碍物
如果所有这些都失败了，机器人将认为它的目标是不可行的，并通知用户它已经中止。
可以使用recovery_behaviour参数配置这些恢复行为，并使用 recovery_behavior_enabled 参数禁用这些恢复行为。
navigation包中有3个recovery相关：
-
clear_costmap_recovery：实现了清除代价地图的恢复行为
遍历所有层，然后如果某层在可清理列表里就清理掉它的 costmap 。默认可清理列表里只有 obstacle layer ，也就是实时扫描建立的 costmap 。
-
rotate_recovery：实现了旋转的恢复行为， 360度旋转来尝试清除空间
转一圈看看有没有路。在
runBehavior里只需要发指令让小车转一圈，有没有路是 local costmap 在转一圈过程中建立发现的。 -
move_slow_and_clear：实现了缓慢移动的恢复行为
清理 costmap 然后什么都不管，按照前进速度和转角速度走。从代码里可以看到，根据指定的距离，这是通过先清除全局 costmap 跟局部 costmap 一圈的 obstacle layer 的障碍，然后直接发指令实现的。由于只清除了 obstacle layer ，其实 static layer 的障碍还在，而且这里清不清除跟发指令关系不大，该撞上去的还是会撞的，相当于闭着眼睛往前走。
参考
ros navigation stack源码学习
turtlebot3-github
创客智造-navigation
navigation
robot_localization Map, Odom, and drift.
ROS: global_planner 整体解析
mpc_local_planner
ROS基础教程–CostMap_2D包的一些理解
Basic Navigation Tuning Guide
通用参数配置文件costmap_common_params.yaml 导航配置文件参数详细解析 （二）
ROS : Navigation - recovery behavior 源码分析