ROS导航包总结
nav_core 定义了整个导航接口，并提供 3 个接口：
- nav_core::BaseGlobalPlanner
- 开源包：global_planner、navfn、carrot_planner
- initialize (std::string name, costmap_2d::Costmap2DROS *costmap_ros)=0
- makePlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)=0
- makePlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan, double &cost)
- nav_core::BaseLocalPlanner
- 开源包：base_local_planner、dwa_local_planner、eband_local_planner、teb_local_planner、mpc_local_planner
- initialize (std::string name, tf2_ros::Buffer tf, costmap_2d::Costmap2DROScostmap_ros)=0
- 创建 Node Handle
- 加载参数
- 障碍物容器
- 创建可视化对象
- 加载 footprint
- 创建局部规划对象
- 初始化 tf、costmap、坐标系
- 设置 odom 话题获得速度
- 验证 footprint 的有效性
- 订阅话题
- computeVelocityCommands (geometry_msgs::Twist &cmd_vel)=0
- 确认是否已初始化
- 获取机器人位姿
- 获取机器人速度
- 裁剪全局路径
- 转换全局路径
- 从全局路径中提取航迹点
- 检查是否到达
- 检查是否振荡、恢复模式
- 获取当前目标
- 起始点处理
- 清除后更新障碍物
- 执行局部路径规划
- 初始化 TEB 轨迹
- optimizeTEB（图优化）
- 构建
- 添加顶点：Pose + TimeDiff
- 添加边（约束）：障碍物、航迹点、速度、加速度、时间最优、路径最短
- 优化
- 清理
- 构建
- 失败则重新初始化
- 检查轨迹前几个姿态的有效性（不发生碰撞）
- 计算速度指令
- 速度限幅处理
- 发布可视化数据：障碍物、航迹点、全局路径
- isGoalReached ()=0
- setPlan (const std::vector< geometry_msgs::PoseStamped > &plan)=0
- nav_core::RecoveryBehavior
- 开源包：clear_costmap_recovery、rotate_recovery
- initialize (std::string name, tf2_ros::Buffer tf, costmap_2d::Costmap2DROSglobal_costmap, costmap_2d::Costmap2DROS *local_costmap)=0
- runBehavior ()=0
move_base 协调感知、定位、目标信息，最终输出控制指令，相关函数如下。
┌────────────────────┐
│ ┌───┴──────────────┐ ┌──────────────────┐
│ │ BaseGlobalPlanner◁────┤ GlobalPlanner │
│ └───┬──────────────┘ └──────────────────┘
│ ┌───┴────────────┐ ┌──────────────────┐
│ │ BaseLocalPlanner◁─────┤ TebLocalPlannerROS
│ MoveBase └───┬────────────┘ └──────────────────┘
│ ┌ ─ ┴ ─ ─ ─ ─ ─ ─ ┐
│ RecoveryBehavior
│ └ ─ ┬ ─ ─ ─ ─ ─ ─ ┘
└──────────◇─────────┘
│
│
┌──────────┴─────────┐
│ ┌───┴────────────┐ ┌──────────────────┐
│ │ Layer │ │ SpatioTemporal │
│ Costmap2DROS │ ◁──────┤ VoxelLayer │
│ └───┬────────────┘ └──────────────────┘
└────────────────────┘
构造函数：
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) {
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
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); // 全局规划的最大次数
private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0); // 振荡超时
private_nh.param("oscillation_distance", oscillation_distance_, 0.5); // 振荡距离
//set up plan triple buffer
planner_plan_ = new std::vector<geometry_msgs::PoseStamped>();
latest_plan_ = new std::vector<geometry_msgs::PoseStamped>();
controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
//set up the planner's thread
planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this)); // 全局规划线程
//for comanding the base
vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );
ros::NodeHandle action_nh("move_base");
action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("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<geometry_msgs::PoseStamped>("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(); // 全局地图暂时停用
//initialize the global planner
try {
planner_ = bgp_loader_.createInstance(global_planner);
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_); // 加载全局规划插件
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what());
exit(1);
}
//create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_); // 局部地图
controller_costmap_ros_->pause(); // 局部地图暂时停用
//create a local planner
try {
tc_ = blp_loader_.createInstance(local_planner);
ROS_INFO("Created local_planner %s", local_planner.c_str());
tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);// 加载局部规划插件
} catch (const pluginlib::PluginlibException& ex) {
ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what());
exit(1);
}
// Start actively updating costmaps based on sensor data
planner_costmap_ros_->start(); // 启用全局地图
controller_costmap_ros_->start(); // 启用局部地图
//advertise a service for getting a plan
make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
//advertise a service for clearing the costmaps
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
//if we shutdown our costmaps when we're deactivated... we'll do that now
if(shutdown_costmaps_){
ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
planner_costmap_ros_->stop(); // 停用全局地图
controller_costmap_ros_->stop(); // 停用局部地图
}
//load any user specified recovery behaviors, and if that fails load the defaults
if(!loadRecoveryBehaviors(private_nh)){ // 加载恢复行为
loadDefaultRecoveryBehaviors();
}
//initially, we'll need to make a plan
state_ = PLANNING; // 初始化为 PLANNING 状态
//we'll start executing recovery behaviors at the beginning of our list
recovery_index_ = 0;
//we're all set up now so we can start the action server
as_->start();
dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
}
清除障碍物信息：
void MoveBase::clearCostmapWindows(double size_x, double size_y){
tf::Stamped<tf::Pose> global_pose;
//clear the planner's costmap
planner_costmap_ros_->getRobotPose(global_pose);
std::vector<geometry_msgs::Point> clear_poly; // 保存局部地图的四个顶点
double x = global_pose.getOrigin().x(); // 机器人坐标 X
double y = global_pose.getOrigin().y(); // 机器人坐标 Y
geometry_msgs::Point pt;
pt.x = x - size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);
pt.x = x + size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);
pt.x = x + size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);
pt.x = x - size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);
planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE); // 清除全局地图的障碍物
//clear the controller's costmap
controller_costmap_ros_->getRobotPose(global_pose);
clear_poly.clear();
x = global_pose.getOrigin().x();
y = global_pose.getOrigin().y();
pt.x = x - size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);
pt.x = x + size_x / 2;
pt.y = y - size_y / 2;
clear_poly.push_back(pt);
pt.x = x + size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);
pt.x = x - size_x / 2;
pt.y = y + size_y / 2;
clear_poly.push_back(pt);
controller_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE); // 清除局部地图的障碍物
}
规划服务函数：
bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){
if(as_->isActive()){
ROS_ERROR("move_base must be in an inactive state to make a plan for an external user");
return false;
}
//make sure we have a costmap for our planner
if(planner_costmap_ros_ == NULL){
ROS_ERROR("move_base cannot make a plan for you because it doesn't have a costmap");
return false;
}
geometry_msgs::PoseStamped start; // 读取全局路径规划的起点坐标
//if the user does not specify a start pose, identified by an empty frame id, then use the robot's pose
if(req.start.header.frame_id.empty())
{
tf::Stamped<tf::Pose> global_pose;
if(!planner_costmap_ros_->getRobotPose(global_pose)){
ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
return false;
}
tf::poseStampedTFToMsg(global_pose, start);
}
else
{
start = req.start;
}
//update the copy of the costmap the planner uses
clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_);
//first try to make a plan to the exact desired goal
std::vector<geometry_msgs::PoseStamped> global_plan;
if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){
ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance",
req.goal.pose.position.x, req.goal.pose.position.y);
//search outwards for a feasible goal within the specified tolerance // 全局路径规划终点设置
geometry_msgs::PoseStamped p; // 路径规划失败后的处理流程
p = req.goal;
bool found_legal = false; // 新的终点是否可行
float resolution = planner_costmap_ros_->getCostmap()->getResolution(); // 地图分辨率
float search_increment = resolution*3.0;
if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;
for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) {
for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) {
for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) {
//don't search again inside the current outer layer
if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue; // 一层层向外搜索
//search to both sides of the desired goal
for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) {
//if one of the offsets is 0, -1*0 is still 0 (so get rid of one of the two)
if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue;
for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) {
if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue;
p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;
if(planner_->makePlan(start, p, global_plan)){
if(!global_plan.empty()){
//adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there
//(the reachable goal should have been added by the global planner)
global_plan.push_back(req.goal);
found_legal = true;
ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
break;
}
}
else{
ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
}
}
}
}
}
}
}
//copy the plan into a message to send out
resp.plan.poses.resize(global_plan.size());
for(unsigned int i = 0; i < global_plan.size(); ++i){
resp.plan.poses[i] = global_plan[i];
}
return true;
}
TEB 调试经验
- 机器人容易靠近墙/角
- 增加膨胀半径
- TEB不考虑膨胀半径
- 执行全局规划的路径
- 默认是时间最短
- 降低
max_global_plan_lookahead_dist，但是降低了避障能力，不推荐 - 推荐 Following the Global Plan (Via-Points)
feasibility_no_of_posesmin_obstacle_distinflation_distmin_obstacle_dist<inflation_dist- 满足条件 width_of_robot + 2*min_obstacle_dist < corridor width
In the first case, you placed the goal inside the local costmap and if
allow_init_with_backwards_motion = true, the optimizer is likely to find the backward motion directly. However, for goals outside the local costmap, intermediate goal poses are obtained from the global plan either close to the costmap border or as soon asmax_global_plan_lookahead_distis exceeded.
In general, we need to assume that the global planner already provides knowledge on intermediate robot orientations. Just imagine a car park, in which parking lots close to the walls can only be accessed by backward motions. However, guessing this from just the local costmap which does not reveal the existence/shape of the parking lot at first is often impossible. However, since not all global planners provide information on the orientation,
global_plan_overwrite_orientation=trueis just a workaround to guess the orientation. And the most likely guess is to drive forward if not any further information like path lengths, global costmap etc. is considered. In case the goal is inside the local costmap it should work out of the box. Otherwise, it is up to the global planner how intermediate orientations are chosen.
TEB turns around multiple times while going towards goal
The global planner does not provide any information on the orientation by default. So the planner specifies some heuristics and usually assumes forward motions (see the link). This could explain your first scenario. However, in the second scenario, the optimizer might choose this solution as time-optimal because the backward speed is quite large (actually similar to the forward speed). Since your linear acceleration is large as well, switching directions could be faster than steering around the corner with smaller angular velocity.
全局规划(global_planner, navfn)方位（Orientation filter）输出的模式：
- Global planner ROS - Orientation filter
- 动态参数：
~orientation_mode:- None=0 (No orientations added except goal orientation)
- Forward=1 (Positive x axis points along path, except for the goal orientation)
- 调整方向后，直行过去
- Interpolate=2 (Orientations are a linear blend of start and goal pose)
- ForwardThenInterpolate=3 (Forward orientation until last straightaway, then a linear blend until the goal pose)
- Backward=4 (Negative x axis points along the path, except for the goal orientation)
- Leftward=5 (Positive y axis points along the path, except for the goal orientation)
- Rightward=6 (Negative y axis points along the path, except for the goal orientation)
TEB 全部参数：
- 机器人配置参数
- acc_lim_x：最大位移加速度
- acc_lim_theta：最大角加速度
- max_vel_x
- max_vel_x_backwards
- max_vel_theta：最大角速度
- 仅用于阿克曼小车的参数
- min_turning_radius
- wheelbase
- cmd_angle_instead_rotvel
- 仅用于全向机器人的参数
- max_vel_y：最大侧向速度
- acc_lim_y：最大侧向加速度
- footprint_model
- type
- radius：仅用于“圆”类型
- line_start：仅用于“线”类型
- line_end：仅用于“线”类型
- front_offset：仅用于“two_circles”类型
- front_radius：仅用于“two_circles”类型
- rear_offset：仅用于“two_circles”类型
- rear_radius：仅用于“two_circles”类型
- vertices：：仅用于“polygon”类型
- is_footprint_dynamic
- 目标阈值参数
- xy_goal_tolerance
- yaw_goal_tolerance
- free_goal_vel
- 轨迹参数
- dt_ref：轨迹的时间分辨率
- dt_hysteresis：时间调整区间，最终时间分辨率的范围调整为
dt_ref+-dt_hysteresis - min_samples：最小采样数
- global_plan_overwrite_orientation：考虑局部代价地图外的Goal，局部规划结果的orientation用全局规划代替
- global_plan_viapoint_sep：全局路径跟踪模式
- max_global_plan_lookahead_dist：每次优化时需考虑的全局规划片段
- force_reinit_new_goal_dist：？？？
- feasibility_check_no_poses：预测位姿的数量
- publish_feedback：用于调试
- shrink_horizon_backup：缩短时间分辨率？
- allow_init_with_backwards_motion：在局部代价地图范围内允许直接后退达到目标点，用于能感知后方的机器人
- exact_arc_length
- shrink_horizon_min_duration
- 障碍物参数
- min_obstacle_dist：机器人离障碍物的最短距离
- include_costmap_obstacles：引入代价地图的障碍物
- costmap_obstacles_behind_robot_dist：只考虑此范围内的障碍物
- obstacle_poses_affected：障碍物关联位姿的数量
- inflation_dist (> min_obstacle_dist)：作为缓冲区
- include_dynamic_obstacles：考虑动态障碍物（假设障碍物的速度不变）
- legacy_obstacle_association (bool, default: false)
- obstacle_association_force_inclusion_factor：图优化时考虑 [value] * min_obstacle_dist 范围内的障碍物
- obstacle_association_cutoff_factor：图优化时不考虑[value] * min_obstacle_dist 范围外的障碍物
- 使能 costmap_converter 时：
- costmap_converter_plugin
- costmap_converter_spin_thread
- costmap_converter_rate：频率应小于代价地图的更新频率
- 优化参数
- no_inner_iterations：内层迭代次数
- no_outer_iterations：外层迭代次数
- penalty_epsilon
- weight_max_vel_x
- weight_max_vel_theta
- weight_acc_lim_x
- weight_acc_lim_theta
- weight_kinematics_nh：全向机器人对应数值调小
- weight_kinematics_forward_drive：数值调小允许后退
weight_kinematics_turning_radius：只用于阿克曼结构- weight_optimaltime
- weight_obstacle
- weight_viapoint
- weight_inflation
- weight_adapt_factor
- 同论路径的并行规划
- enable_homotopy_class_planning
- enable_multithreading
- max_number_classes：并行规划路径时的最大条数
- selection_cost_hysteresis：new_cost < old_cost * [value])
- selection_obst_cost_scale
- selection_viapoint_cost_scale
- selection_alternative_time_cost
- roadmap_graph_no_samples
- roadmap_graph_area_width
- h_signature_prescaler
- h_signature_threshold
- obstacle_heading_threshold
- visualize_hc_graph
- viapoints_all_candidates
- switching_blocking_period
- 相关参数
- odom_topic
- map_frame：全局规划的坐标系