在TEB中的footprint模型 中提到calculateDistance函数计算机器人到障碍物的 Euclidean 距离，用于图优化环节。

以最简单的Point类型为例

1
2
3
4
5

virtual double calculateDistance(const PoseSE2& current_pose, 
    const Obstacle* obstacle) const
{
	return obstacle->getMinimumDistance(current_pose.position());
}

1
2
3
4
5

PointObstacle
CircularObstacle
PillObstacle
LineObstacle
PolygonObstacle

createGraph() —— PointObstacle::checkLineIntersection —— PointObstacle::checkCollision

TEB算法中的footprint模型是单独构建的，而没有直接调用通用代价地图中的 footprint，二者footprint模型不同，但在TEB中二者都有使用。

LayeredCostmap类的成员函数： double getInscribedRadius() { return inscribed_radius_; }

对于polygon轮廓，两种footprint模型的内接半径相同， TEB的模型没有返回外接半径的函数

在TebLocalPlannerROS::initialize中定义: RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh);，然后进行模型验证validateFootprints

在planner初始化时，也就是在TebOptimalPlanner::initialize中赋值给robot_model_.

很显然BaseRobotFootprintModel是个抽象类，定义了机器人footprint/contour models的接口，根据yaml设置的类型而定义相应的类，这个类只用于optimization，代价地图的footprint只用于检查路径的可行性

• Point 和 circular模型的footprints要求算力最少。

• two-circles模型要求的是两次 distance calculations

• line模型增加了一些 simple case checking, 但是算力在大多数情况仍要求不高。

• polygon模型对每一个edge都要求 distance check，所以计算时间主要取决于 edges和vertices 的数量

也就是说这几种模型的算力要求依次增大。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

RobotFootprintModelPtr TebLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh)
{
  std::string model_name; 
  if (!nh.getParam("footprint_model/type", model_name))
  {
	// 如果未指定类型，则使用点类型
    return boost::make_shared<PointRobotFootprint>();
  }
  // point  
  if (model_name.compare("point") == 0)
  {
      ROS_INFO("Footprint model 'point' loaded for trajectory optimization.");
      return  boost::make_shared<PointRobotFootprint>();
  }
  // circular   省略
  if (model_name.compare("circular") == 0)
  // line  省略
  if (model_name.compare("line") == 0)
  // two circles 省略
  if (model_name.compare("two_circles") == 0)

  // polygon  只对 Point2dContainer vertices_; 赋值
  if (model_name.compare("polygon") == 0)
  {
    // check parameters
    XmlRpc::XmlRpcValue footprint_xmlrpc;
    if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc) )
    {
        ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for 
      	trajectory optimization, since param '" << nh.getNamespace() << 
      	"/footprint_model/vertices' does not exist. Using point-model instead");
        return boost::make_shared<PointRobotFootprint>();
    }
    // get vertices
    if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
    {
      try
      {
        Point2dContainer polygon = makeFootprintFromXMLRPC(footprint_xmlrpc, 
        	"/footprint_model/vertices");
        ROS_INFO_STREAM("Footprint model 'polygon' loaded for trajectory optimization.");
        return boost::make_shared<PolygonRobotFootprint>(polygon);
      } 
      catch(const std::exception& ex)
      {
        ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization: "
        	<< ex.what() << ". Using point-model instead.");
        return boost::make_shared<PointRobotFootprint>();
      }
    }
    else
    {
      ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, 
      	since param '" << nh.getNamespace() << "/footprint_model/vertices' does not 
      	define an array of coordinates. Using point-model instead.");
      return boost::make_shared<PointRobotFootprint>();
    }
  }
  return boost::make_shared<PointRobotFootprint>();
}

validateFootprints

1
2
3
4
5
6
7

// TebLocalPlannerROS::initialize中的调用
validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, 
          cfg_.obstacles.min_obstacle_dist);

void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, 
    double costmap_inscribed_radius, double min_obst_dist)
{}

函数就一句ROS_WARN_COND，要求 TEB优化所用的机器人内接半径 + 参数 min_obstacle_dist > costmap中的内接半径(包括footprint_padding) ，否则Infeasible optimziation results会频繁出现

这里就是设置为点类型的缺陷之处，第1参数个会是0，但是如果min_obstacle_dist超过costmap中的内接半径，会无法走窄通道。

BaseRobotFootprintModel的派生类实现了calculateDistance函数，计算机器人到障碍物的 Euclidean距离，用于图优化环节。以最简单的Point类型为例

1
2
3
4
5

virtual double calculateDistance(const PoseSE2& current_pose, 
    const Obstacle* obstacle) const
{
	return obstacle->getMinimumDistance(current_pose.position());
}

变量robot_model_在之后用于图优化时的一系列函数：
AddEdgesObstacles
AddEdgesObstaclesLegacy
AddEdgesDynamicObstacles
AddEdgesVelocityObstacleRatio，也就是所有和障碍相关的约束

以及可视化中的publishInfeasibleRobotPose

配置使用的是costmap_converter::CostmapToLinesDBSRANSAC，类继承关系:

1

class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons

这个机制太复杂了，还没有搞清楚

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

void workerCallback(const ros::TimerEvent&)
{
  updateCostmap2D();
  compute();
}


void CostmapToPolygonsDBSMCCH::updateCostmap2D()
{
	occupied_cells_.clear();
	if (!costmap_->getMutex())
	{
		ROS_ERROR("Cannot update costmap since the mutex pointer is null");
		return;
	}
	int idx = 0;
	costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());
	    
	// get indices of obstacle cells
	for(std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)
	{
	for(std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)
	{
	  int value = costmap_->getCost(i,j);
	  if(value >= costmap_2d::LETHAL_OBSTACLE)
	  {
	    double x, y;
	    costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
	    occupied_cells_.push_back( KeyPoint( x, y ) );
	  }
	  ++idx;
	}
	}
}

TEB默认情况下不使用Costmap Converter。事实上，此插件可以在复杂场景下极大提高运算效率，尤其是处理激光雷达分散的测量数据时，因为将障碍物视为系列孤立点效率极低。

使用costmap_converter转换障碍，结果Rviz没有显示出来，同时报警

首先确认安装sudo apt-get install -y ros-visualization-msgs，一般安装后就行了

初始化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

try
{
    costmap_converter_ = costmap_converter_loader_.createInstance(cfg_.obstacles.costmap_converter_plugin);
    std::string converter_name = costmap_converter_loader_.getName(cfg_.obstacles.costmap_converter_plugin);
    // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace
    boost::replace_all(converter_name, "::", "/");
    costmap_converter_->setOdomTopic(cfg_.odom_topic);
    costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name));
    costmap_converter_->setCostmap2D(costmap_);
    
    costmap_converter_->startWorker(ros::Rate(cfg_.obstacles.costmap_converter_rate), costmap_, cfg_.obstacles.costmap_converter_spin_thread);
    ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded.");        
}
catch(pluginlib::PluginlibException& ex)
{
    ROS_WARN("The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treated as point obstacles. Error message: %s", ex.what());
    costmap_converter_.reset();
}

配置使用的是costmap_converter::CostmapToLinesDBSRANSAC

computeVelocityCommands 函数中的调用

1
2
3
4

if (costmap_converter_)
  updateObstacleContainerWithCostmapConverter();
else
  updateObstacleContainerWithCostmap();

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter()
{
  if (!costmap_converter_)
    return;
    
  //Get obstacles from costmap converter
  costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles();
  if (!obstacles)
    return;

  for (std::size_t i=0; i<obstacles->obstacles.size(); ++i)
  {
    const costmap_converter::ObstacleMsg* obstacle = &obstacles->obstacles.at(i);
    const geometry_msgs::Polygon* polygon = &obstacle->polygon;

    if (polygon->points.size()==1 && obstacle->radius > 0) // Circle
    {
      obstacles_.push_back(ObstaclePtr(new CircularObstacle(polygon->points[0].x, polygon->points[0].y, obstacle->radius)));
    }
    else if (polygon->points.size()==1) // Point
    {
      obstacles_.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y)));
    }
    else if (polygon->points.size()==2) // Line
    {
      obstacles_.push_back(ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y,
                                                        polygon->points[1].x, polygon->points[1].y )));
    }
    else if (polygon->points.size()>2) // Real polygon
    {
        PolygonObstacle* polyobst = new PolygonObstacle;
        for (std::size_t j=0; j<polygon->points.size(); ++j)
        {
            polyobst->pushBackVertex(polygon->points[j].x, polygon->points[j].y);
        }
        polyobst->finalizePolygon();
        obstacles_.push_back(ObstaclePtr(polyobst));
    }

    // Set velocity, if obstacle is moving
    if(!obstacles_.empty())
      obstacles_.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation);
  }
}

局部代价地图更新频率应当不低于planner和costmap_conveter的转换频率

话题

• global_plan (nav_msgs/Path)

• local_plan (nav_msgs/Path) 局部路径，主要是可视化目的

• teb_poses (geometry_msgs/PoseArray) 当前本地计划的离散姿态列表(SE2)，主要用于可视化目的， 个数是 teb.sizePoses() ，每个位姿点其实就是teb.Pose(i)

这两个话题本质都是一样的，发布在TebVisualization::publishLocalPlanAndPoses

• teb_markers (visualization_msgs/Marker) teb_local_planner通过具有不同名称空间的标记来提供规划场景的其他信息。 Namespaces PointObstacles and PolyObstacles: visualize all point and polygon obstacles that are currently considered during optimization. Namespace TebContainer: Visualize all found and optimized trajectories that rest in alternative topologies (only if parallel planning is enabled). Some more information is published such as the optimization footprint model

话题teb_markers_array 和 eb_markers是绑定的，不过不必关注。

话题的发布在TebVisualization::publishRobotFootprintModel

• teb_feedback (teb_local_planner/FeedbackMsg)
  反馈消息包含计划的轨迹，包括速度轮廓和时间信息以及障碍物列表。主要用于评估和调试。必须启用参数publish_feedback

话题的发布在TebVisualization::publishFeedbackMessage

参数

某些参数不会加载到参数服务器，需要用户在yaml里设置，这是源码决定的

速度

• max_vel_x: 1.0

• max_vel_x_backwards: 0.5 最大倒车速度。将此速度设置为0或者负数将导致错误。禁止倒车应在penalty部分将前向行驶的权重设置得极高。 参阅优化参数weight_kinematics_forward_drive

• acc_lim_x: 0.5 最大线加速度，m²/s

• max_vel_theta: 1 最大转向角速度，作用在车辆高速行驶时，防止高重心转弯过快翻车

• acc_lim_theta: 0.3 角加速度限制可以避免车轮打滑，车辆失控。对于低速平稳运行的车辆可以不约束此两项，将二者设为一个足够大的值即可

• max_vel_y: 0.2

• acc_lim_y: 0.5

以上几个最大值约束会用于saturateVelocity函数

• min_turning_radius: 0.0 最小转弯半径，对于差动机器人为0，代表可以原地转弯。源码在TebLocalPlannerROS::convertTransRotVelToSteeringAngle， 转弯半径应在低速时生效. 把这个值稍微调大一点，可以防止车采用过分的差速转向策略，而导致速度非常缓慢。 转向半径肯定在一米以内，可以按需设置，转向半径如果大了，U形弯就会外道入弯，能比较轻松过弯，不过小弯道的时候也会贴外侧，这样会比较浪费时间，但是如果小了，小弯会贴内，节省时间，但是U形弯就会内侧入弯，那就有可能过不了弯了。
  这个参数让TEB更适合阿克曼模型。为避免规划出不可能实现的移动路径，请动手测量实际车辆的转弯半径。此参数事实上约束了舵机的最大转角。有些车辆转向性能不佳，前轮实际转过角度小于舵机角度，则应当给指令转角乘上一增益后再控制舵机，否则车辆将总是不能实现设置的最小转弯半径。请注意此项应当与最大角速度配合设置。

• cmd_angle_instead_rotvel: false # not used, is differential 是否将收到的角速度消息转换为操作上的角度变化。设置成True时，话题cmd_vel.angular.z内的数据是舵机角度。

cmd_angle_instead_rotvel 和 min_turning_radius不能同时为0，否则报警 You are mixing a carlike and a diffdrive robot

• wheelbase: 默认0.0，目前是1，只有在cmd_angle_instead_rotvel为true时才有效

轮廓

• footprint_model: 可用的有 point, circular, two_circles, line, polygon. point是最简单的，所需的计算时间最少，但准确度也最低；polygon多边形模型最复杂，完全考虑到机器人的轮廓形状，计算准确度最高。circular类型把机器人建模为一个简单的圆，半径为/footprint_model/radius，计算距离和point相似，不同点是每次调用函数时机器人半径会被加入到参数min_obstacle_dist。对圆形轮廓，可以因此选择point类型，然后将半径加到最小障碍物距离来替换，减小计算量

• is_footprint_dynamic: 目前False，是否footprint为动态的. If true, 在检查trajectory feasibility之前更新footprint

GoalTolerance

• xy_goal_tolerance: 0.1
• yaw_goal_tolerance: 0.05
• free_goal_vel: False 自由目标速度。设为False时，车辆到达终点时的目标速度为0。缺少目标速度约束将导致车辆“全速冲线”，即使前方有一堵墙也是如此 (因为撞墙的时刻不在规划器考虑范围内了)

Trajectory

• teb_autosize: True 优化期间允许改变轨迹的时域长度，也就是改变 dt_ref

• dt_ref: 0.3 局部路径规划的解析度; Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)。 TEB通过状态搜索树寻找最优路径，而dt_ref则是最优路径上的两个相邻姿态（即位置、速度、航向信息，可通过TEB可视化在rivz中看到）的默认距离。 此距离不固定，规划器自动根据速度大小调整这一距离，速度越大，相邻距离自然越大，较小的值理论上可提供更高精度。
  从autoResize函数可以看到，当相邻时间差距离(TimeDiff(i) )和dt_ref的差超过正负dt_hysteresis时，规划器将改变这一距离。 增大到0.4，teb_pose个数大大减少，车速明显加快，多数接近最大速度

• dt_hysteresis: 0.1 允许改变的时域解析度的浮动范围， 一般为 dt_ref 的 10% 左右

• min_samples: 3，必须大于2。 源码在initTrajectoryToGoal和updateAndPruneTEB以及autoResize

• max_samples: 500. 源码在optimizeTEB中的autoResize

• allow_init_with_backwards_motion: 默认true，设置为false. 只影响初始化，而不是路径优化的结果。 源码在initTrajectoryToGoal，只有使用的TebOptimalPlanner::plan第一个参数是initial_plan才有效

• force_reinit_new_goal_dist: 1.0 源码在TebOptimalPlanner::plan的热启动阶段，判断局部目标(transformed_plan最后一个元素)与teb_poses最后一个元素之差的范数是否达到阈值，其实是判断局部目标是否太远了。如果小于阈值则调用updateAndPruneTEB

• global_plan_overwrite_orientation: True 覆盖全局路径中的临时局部路径点的朝向，有些全局规划器不考虑局部子目标的朝向，因此在这里校正。将未来一段的规划的姿态角的平均值作为估计的局部子目标的姿态。源码estimateLocalGoalOrientation和initTrajectoryToGoal

• global_plan_viapoint_sep: 0.3 # 正数才启用。调整全局路径上选取的航迹点的间隔，应根据机器人的尺寸大小调整.

• max_global_plan_lookahead_dist: 1.5 最大向前看距离. 指定要进行优化的全局计划子集的最大长度 (累积欧几里得距离) 。然后，实际长度由本地成本图大小和此最大界限的逻辑结合确定。设置为零或负数以停用此限制。 此距离 1.应随车辆最大速度的增大而增大 2.不应超过激光雷达等传感器的可靠测量范围 3. 不应超过局部代价地图的大小，即不能要求TEB对局部耗费地图以外的部分进行规划。

在TebLocalPlannerROS::transformGlobalPlan()函数中被使用，决定局部规划初始轨迹的最大长度，实际调试发现此参数无需过大，因为局部轨迹在每个控制周期都被更新，实际执行的指令仅是轨迹上第一个点的速度值，这里设置为1.5即可， 过长也可能导致优化结果无法有效收敛

• global_plan_prune_distance: 0.6 在TebLocalPlannerROS::pruneGlobalPlan()函数中被使用，因为全局路径是从全局起始点到全局目标点的一条轨迹，而初始的局部路径仅是从机器人当前位置到局部目标点的一小段路径，全局路径裁剪其中一部分即局部路径，该参数决定了从机器人当前位置的后面一定距离开始裁剪

• force_reinit_new_goal_angular: 0.78 原理同上，不过是角度，弧度制

  1 2 3 4

  if ( teb_.sizePoses()>0 && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta()) ) < cfg_->trajectory.force_reinit_new_goal_angular )

• feasibility_check_no_poses: 3 在判断生成的轨迹是否冲突时使用，此时设置为3，即从轨迹起点开始逐个检查轨迹上的3个点，若3个点均不发生碰撞，则认为本次轨迹有效。若小于0则检查所有路径点。由于teb优化并非硬约束，这里相当于是轨迹生成之后的一层保障， 这个参数根据机器人的速度和环境复杂程度调整，否则极有可能出现在狭窄环境中走走停停的情况，甚至可以减小到1或0 源码在 isTrajectoryFeasible。 如果isTrajectoryFeasible函数检查的局部路径点太多，有的会超出局部代价地图，结果函数一直返回false

• min_resolution_collision_check_angular: 默认不加载，需要用户赋值，比如 3.14159265。代价地图的检查碰撞所用的最小角分辨率。如果未达到要求，将增加 intermediate samples. 源码在TebOptimalPlanner::isTrajectoryFeasible

• control_look_ahead_poses 非默认参数，需要用户赋值，比如 2。 ROS Wiki对这个参数的解释其实是min_resolution_collision_check_angular

• exact_arc_length: false. 一般用true. 如果为true，则在速度，加速度和转弯速率计算中使用精确的弧长(计算三角函数，会增加cpu负担)，否则使用两个位姿点的直线距离做近似。

可视化

• publish_feedback: false. 发布包含完整轨迹和活动障碍物的反馈，在可视化部分，对应TebOptimalPlanner::visualize()

• visualize_with_time_as_z_axis_scale: 局部路径的z坐标赋值，源码在publishLocalPlanAndPoses， TebVisualization::publishObstacles

恢复措施

恢复措施可以尝试将快要撞到障碍或在空隙间不走的机器人或路径规划错误的机器人恢复至正常状态。TEB实现了由move_base规定的振荡恢复方法。 但是，实测中控制器将高频率振荡速度指令(>10Hz)，用户应当评估自己的电动机能否承受。 (可在 move_base 配置中关闭)

• shrink_horizon_backup: true. 在自动检测到问题(plan not feasible)的情况下，允许planner临时缩小范围(50%). TEB将以更近的点作为规划目标，尝试重新规划出可行路径。调试时可关闭，以在可视化界面上观察原出错路径。另请参阅参数shrink_horizo​​n_min_duration

• shrink_horizon_min_duration: 10. 如果检测到不可行的轨迹，激活缩小的水平线后备模式，本参数为其最短持续时间。个人怀疑这个参数无意义，因为它更新值的地方也都有no_infeasible_plans_变量的更新，判断时，只根据no_infeasible_plans_即可，详见代码configureBackupModes

• oscillation_recovery: 默认true. Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards

• oscillation_recovery_min_duration: 10 在这个时间内，是否再次发生FailureDetector检测的振荡，调用在configureBackupModes中

在小车遇到空隙不向前走时，容易出现下面的信息，然后小车来回振荡，有时能导航成功

1
2
3
4
5

[ WARN] Clearing costmap to unstuck robot (3.00 m).
[ WARN] TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] Rotate recovery behavior started.   # 开始振荡  RotateRecovery::runBehavior()
[ INFO] TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).

• oscillation_filter_duration: 10 failure_detector_中buffer容器的大小是oscillation_filter_duration * controller_frequency)

• oscillation_v_eps: (0,1)内的 normalized 线速度的平均值的阈值，默认0.1

• oscillation_omega_eps: (0,1)内的 normalized 角速度的平均值，默认0.1

Obstacles

• min_obstacle_dist: 0.05 最小障碍物距离，用于限制机器人与障碍物的最小距离，默认0.5。若设置了车辆的轮廓，则不建议使用此参数。使用车辆轮廓配置footprint_model配合膨胀距离即可实现较好效果。障碍物惩罚权重很高，TEB不会违背这一最小距离约束，因此会把可以通过的缝隙视作不能通过，并终止运行。
  增大min_obstacle_dist可以防止机器人离墙太近，但是对于狭窄通道的情况，反而应该设置的很小。 可以增大机器人轮廓，但减小min_obs_dist，提高防撞性，又保证过窄通道

实际还配合obstacle_association_force_inclusion_factor 和 obstacle_association_cutoff_factor两个参数生效， 参考TebOptimalPlanner::AddEdgesObstacles()函数

• obstacle_association_force_inclusion_factor: 10.0 the obstacles that will be taken into account are those closer than min_obstacle_dist * factor, if legacy is false

• obstacle_association_cutoff_factor: 40.0 the obstacles that are further than min_obstacle_dist * factor will not be taken into account, if legacy is false

距离小于min_obstacle_dist * obstacle_association_force_inclusion_factor值的障碍物点，被强制加入优化框架中，距离大于min_obstacle_dist * obstacle_association_cutoff_factor的障碍物点被直接抛弃不再考虑，然后在剩余的障碍物点中计算机器人左侧最小距离和右侧最小距离。这三个参数的设置非常重要，需要根据机器人的外形尺寸小心调整，否则极易出现狭窄空间机器人无法通过或优化不收敛的情况。

• inflation_dist: 默认0.0 障碍物膨胀距离。这个值必须大于min_obstacle_dist才有效。源码在AddEdgesObstacles. 此膨胀只是降低通过这些区域的优先级，不应当用此距离迫使车辆远离障碍物。障碍物周边的缓冲区，零惩罚代价，缓冲区会导致规划器减速，源码在AddEdgesObstacles，另外参考weight_inflation
  
  

• include_costmap_obstacles: True 这个不是True就不用干了

• costmap_obstacles_behind_robot_dist: 1.0 # distance at which obstacles behind the robot are taken into account

• legacy_obstacle_association: false 源码在buildGraph

• obstacle_poses_affected: 25 对于每一个障碍物（点、线、多边形），距其最近的poses都会被定位出来。参数obstacle_poses_affected个相邻的最近poses都会被考虑进来。略微影响障碍物周围轨迹的平滑度。 如果legacy_obstacle_association是false，则不启用

costmap_converter

• costmap_converter_plugin: 可取值为costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::CostmapToPolygonsDBSConcaveHull, “” 空字符串表示不启用

目前所用为CostmapToLinesDBSRANSAC"，头文件costmap_to_lines_ransac.h，最终的基类是BaseCostmapToPolygons

决定是否使用costmap_converter插件，原始costmap中障碍物全部以点来表示，计算机器人到障碍物的距离实际需要计算机器人到每一个障碍物点的距离，当环境非常复杂时计算代价会非常大。costmap_converter插件的作用是将障碍物预先表示成线段或多边形的形式，可以在一定程度上减轻后续计算距离的压力，尤其是处理激光雷达分散的测量数据时。因为将障碍物视为系列孤立点效率极低。 “costmap_converter::CostmapToPolygonsDBSMCCH”是一个较为精确的方法，它将环境转换为非凸多边形；在将障碍物距离加入g2o优化框架中 (障碍物距离是目标函数之一，描述为超图的边)

• costmap_converter_spin_thread: True
• costmap_converter_rate: 10

Optimization

除了撞击障碍物不被允许外，其余的约束在没有可行方案时可被打破（如规划出事实上不可行的转弯半径）

• no_inner_iterations: 5 图优化optimizer的迭代次数

• no_outer_iterations: 4 每次外部循环迭代都会根据所需的时间分辨率dt_ref自动调整轨迹的大小，并调用内部优化器

• penalty_epsilon 0.1 一次性改变所有的惩罚项，为惩罚函数增加一个小的安全余量，以实现硬约束近似. 例如为速度的约束提供一个缓冲的效果，在到达速度限制前会产生一定的惩罚，让其提前减速达到缓冲的效果
  

  penalty_epsilon=0.1.png
  
  
  penalty_epsilon=0.5.png
  
  
  0.5时报警.png

如果选择过高的值，会影响最终的信息矩阵，可能会出现不收敛的情况

• weight_max_vel_x 2 满足最大允许平移速度的优化权重，还有最大角速度权重，以及加速度，角加速度权重，在整个运动过程中以主要以高速还是低速运行，则是看这些权重的分配
• weight_max_vel_theta (double，默认值：1.0) 满足最大允许角速度的优化权重
• weight_acc_lim_x (double，默认值：1.0) 满足最大允许平移加速度的优化权重

• weight_acc_lim_theta (double，默认值：1.0) 满足最大允许角加速度的优化重量

• weight_multiplier

• weight_obstacle 默认值：50.0. 优化权重以保持与障碍物的最小距离。可以增大至几百，让机器人提前转向，避免卡死

• obstacle_cost_exponent 无默认值，目前设置为0.65. 源码在EdgeObstacle::computeError和EdgeInflatedObstacle::computeError，判断是否为1，同时min_obstacle_dist > 0才有效。用于更新g2o的误差函数

• weight_kinematics_nh 默认值：1000. 用于满足非完整运动学的优化权重. 该参数必须很高，因为运动学方程构成了一个等式约束，即使值1000也不意味着由于与其他成本相比较小的“原始”成本值而导致的矩阵条件不佳

• weight_kinematics_forward_drive 默认值：1，改为1000。 迫使机器人仅选择前进方向 (正向速度) 。较小的权重，仍然会允许向后行驶。我们无法限制优化器的搜索空间为正的速度，因为优化器不支持硬约束，只能大幅提高weight_kinematics_forward_drive以避免后退速度，但是值太大又会减小收敛速度。不管权重多高，无法规划出前进的路径时，还是可能出现停滞或者倒车。

• weight_optimaltime 最短时间权重。提高至5时，车辆在直道上线速度明显加快，而且越大越能接近max_vel_x，并靠近路径边缘沿切线过弯。 Optimization weight for contracting the trajectory w.r.t transition time

• weight_inflation 默认0.1， Optimization weight for the inflation penalty，应当很小，源码在AddEdgesObstacles的信息矩阵里

• weight_dynamic_obstacle Optimization weight for satisfying a minimum separation from dynamic obstacles

• weight_dynamic_obstacle_inflation Optimization weight for the inflation penalty of dynamic obstacles (should be small)

• weight_adapt_factor 2. 源码只有optimizeTEB的最后。 每个外部TEB迭代中，一些特殊权重 (当前为weight_obstacle) 会通过此因子重复缩放 (weight_new = weight_old * factor) 。迭代增加权重而不是设置较大的先验值会导致底层优化问题的更好数值条件。

• optimization_verbose: 默认false. 图优化的过程是否显示，可以用于调试

  1 2	iteration= 1 chi2= 0.458692 time= 9.3481e-05 cumTime= 0.000284497 edges= 49 schur= 0 lambda= 5.804560 levenbergIter= 2

并行规划

• enable_homotopy_class_planning (bool，默认：true) 激活并行规划(占用更多的CPU资源) 若在单一路径上运行，则没有必要时用此功能

2021-06-11_174540.png

• enable_multithreading (bool，默认：true) 激活多个线程以计划不同线程中的每个轨迹

• max_number_classes (int，默认值：4，设置为2) 指定要考虑的最大不同轨迹数(限制计算工作量)，影响CPU

• selection_cost_hysteresis (double，默认值：1.0) 指定新候选人必须具有多少轨迹成本才能选择之前的轨迹 (如果new_cost < old_cost * factor，则进行选择

• selection_prefer_initial_plan 默认0.95 Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan

• selection_obst_cost_scale (double，默认值：100.0) 仅选择“最佳”候选者的障碍物成本术语的额外缩放

• selection_viapoint_cost_scale (double，默认值：1.0) 仅为了选择“最佳”候选者而对通孔成本条款进行额外的缩放

• selection_alternative_time_cost (bool，默认值：false) 如果为true，则将时间成本 (时间差平方的总和) 替换为总过渡时间 (时间差之和)

• simple_exploration 默认false. 如果为true，不同的 trajectories are explored using a simple left-right approach (pass each obstacle on the left or right side) for path generation, otherwise sample possible roadmaps randomly in a specified region between start and goal.

• roadmap_graph_no_samples (int，默认值：15) 指定为创建roadmap graph而生成的样本数，前提是simple_exploration为false

• roadmap_graph_area_width (double，默认值：6) 在起点和目标之间的矩形区域中采样随机关键点/航路点。以米为单位指定该区域的宽度

• delete_detours_backwards 默认true. planner will discard the plans detouring backwards with respect to the best plan

• h_signature_prescaler (double，默认值：1.0) 标度内部参数 (H-signature) ，用于区分同伦类。警告：仅当您在本地成本图中观察到障碍物过多的问题时，才减小此参数，请勿将其选得太低，否则障碍物无法彼此区分 (0.2 < 值 <= 1)

• h_signature_threshold (double，默认值：0.1) 如果实部和复杂部的差都低于指定的阈值，则假定两个H签名相等

• obstacle_heading_threshold (double，默认值：1.0) 在障碍物航向和目标航向之间指定标量乘积的值，以便将障碍物)考虑在内进行探索

• visualize_hc_graph (bool，默认：false) 如果为true，在rviz里可看到优化使用的graph

• viapoints_all_candidates (bool，默认：true) 如果为true，则将不同拓扑的所有轨迹附加到该组通孔点，否则，仅将与初始/全局计划共享相同拓扑的轨迹与它们连接 (对test_optim_node无效)

• switching_blocking_period (double，默认值：0.0) 指定允许切换到新的等效类之前需要终止的持续时间 (以秒为单位)

提高性能的调试

• 使用Costmap Converter
• 减小no_outer_iterations no_inner_iterations
• 减小局部地图大小
• 增大dt_ref obstacle_poses_affected
• 关闭多路径并行规划 (效果非常显著)
• 降低 max_lookahead_distance (一般)

如何提高线速度

增大max_vel_x, dt_ref, weight_max_vel_x。如果还不见效就增大 weight_optimaltime

TEB参数设置如下:

1
2
3
4
5
6

dt_ref: 0.5
max_vel_x: 3.0
max_vel_x_backwards: 0.02
max_vel_y: 0.0
acc_lim_x: 0.8
weight_optimaltime: 30

速度实测.png

加大weight_optimaltime到45，最大速度超过了2.8，但修改dt_ref的效果不大了，可见weight_optimaltime提高最大速度的效果比dt_ref更好

尽量避免后退

要求TEB的速度命令只有前进命令是不可能的，因为优化框架不支持硬约束，轻微违反约束是可能的。 应当显著加大weight_kinematics_forward_drive，但是权重太大又会降低收敛速度(这对任何权重都是如此) 。将 max_vel_x_backwards减少到0(或0.02)，至少减少到与参数penalty_epsilon相同。

ROS源码里有个costmap_2d_node.cpp，编译成可执行文件costmap_2d_node，注意它的链接库：

1
2
3
4
5
6

target_link_libraries(costmap_2d_node
    costmap_2d
    ${PCL_LIBRARIES}
    ${Boost_LIBRARIES}
    ${catkin_LIBRARIES}
)

1
2
3
4
5
6
7
8
9
10
11

#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "costmap_node");
  tf::TransformListener tf(ros::Duration(10));
  costmap_2d::Costmap2DROS lcr("costmap", tf);
  ros::spin();
  return (0);
}

获取参数值和tf变换

首先是一些参数的获取。循环等待直到获得机器人底盘坐标系和global系之间的坐标转换。
并获取rolling_window、track_unknown_space、always_send_full_costmap的参数，默认为false。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63

Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) :
    layered_costmap_(NULL),
    name_(name),  // 局部还是全局代价地图
    tf_(tf),
    transform_tolerance_(0.3),
    map_update_thread_shutdown_(false),
    stop_updates_(false),
    initialized_(true),
    stopped_(false),
    robot_stopped_(false),
    map_update_thread_(NULL),
    last_publish_(0),
    plugin_loader_("costmap_2d", "costmap_2d::Layer"),
    publisher_(NULL),
    dsrv_(NULL),
    footprint_padding_(0.0)
{
  //初始化old pose
  old_pose_.setIdentity();
  old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30));
  // 如果是全局代价地图，这里name是global_costmap
  ros::NodeHandle private_nh("~/" + name);
  ros::NodeHandle g_nh;

  //获取tf前缀
  ros::NodeHandle prefix_nh;
  std::string tf_prefix = tf::getPrefixParam(prefix_nh);

  //两个坐标系，全局和局部的参数是不同的
  private_nh.param("global_frame", global_frame_, std::string("/map"));
  private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));

  //确保基于tf前缀正确设置了坐标系
  //tf::resolve或者TransformListener::resolve的作用就是使用tf_prefix参数将frame_name解析为frame_id
  global_frame_ = tf::resolve(tf_prefix, global_frame_);
  robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);

  ros::Time last_error = ros::Time::now();
  std::string tf_error;
  
  //确保机器人global_frame_和robot_base_frame_之间的有效转换, 否则一直停留在这里阻塞
  // 这里已经写死为 0.1，有需要可以改变
  while (ros::ok()
      && !tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), 
        ros::Duration(0.1), ros::Duration(0.01), &tf_error))
  {
    ros::spinOnce();
    // 如果５秒内未成功tf转换，报警
    if (last_error + ros::Duration(5.0) < ros::Time::now())
    {
      ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
               robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
      last_error = ros::Time::now();
    }
    // The error string will accumulate and errors will typically be the same, so the last
    // will do for the warning above. Reset the string here to avoid accumulation.
    tf_error.clear();
  }
  //检测是否需要“窗口滚动”，从参数服务器获取以下参数
  bool rolling_window, track_unknown_space, always_send_full_costmap;
  private_nh.param("rolling_window", rolling_window, false);
  private_nh.param("track_unknown_space", track_unknown_space, false);
  private_nh.param("always_send_full_costmap", always_send_full_costmap, false);

LayeredCostmap类的对象

layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

接下来创建一个LayeredCostmap类实例layered_costmap_，作为规划器使用的master costmap，并通过它管理各层地图。

LayeredCostmap类是Costmap2DROS的类成员，它是master costmap，也能够管理各层地图，因为它 含有指向各层子地图的指针 std::vector<boost::shared_ptr<Layer> > plugins_，能够调用子地图的类方法，开启子地图的更新，各层子地图最后都会合并到master costmap，提供给规划器的使用。

它的构造函数只有:

1
2
3
4
5
6
7
8
9
10
11

// 如果追踪未知信息的cell，默认false
if (track_unknown)
	costmap_.setDefaultValue(255);
else
	costmap_.setDefaultValue(0);

// 枚举值的含义对应如下：
/*	NO_INFORMATION = 255;
	LETHAL_OBSTACLE = 254;
	INSCRIBED_INFLATED_OBSTACLE = 253;
	FREE_SPACE = 0;    */

继续Costmap2DROS的构造函数代码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

// 查看参数服务器里的costmap是否有plugins参数，如果没有，则重置为旧参数
if (!private_nh.hasParam("plugins"))
{
    resetOldParameters(private_nh);
}
// 如果加载了plugins，循环将每个plugin即每层子地图添加进layered_costmap_类的
// 指针组对象中，并对每层地图调用其 initialize 类方法，初始化各层地图。
if (private_nh.hasParam("plugins"))
{
    XmlRpc::XmlRpcValue my_list;
    private_nh.getParam("plugins", my_list);
    for (int32_t i = 0; i < my_list.size(); ++i)
    {
      std::string pname = static_cast<std::string>(my_list[i]["name"]);
      std::string type = static_cast<std::string>(my_list[i]["type"]);
      ROS_INFO("Using plugin \"%s\"", pname.c_str());

      // plugin_loader_类型是　pluginlib::ClassLoader<Layer>，ROS插件机制
      boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
      // 添加到容器  plugins_
      layered_costmap_->addPlugin(plugin);
      // 执行 Layer::initialize， 它向各层地图通知主地图的存在，并调用
      // oninitialize方法  （Layer类中的虚函数，被定义在各层地图中）
      plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
    }
}

footprint话题

订阅footprint话题，回调函数为setUnpaddedRobotFootprintPolygon。当话题上收到footprint时，回调函数会将接收到的footprint根据参数footprint_padding_的值进行膨胀，得到“膨胀”后的padded_footprint_，传递给各级地图。

查看节点可知，/move_base/local_costmap/footprint和/move_base/global_costmap/footprint两个话题的发布订阅者都是move_base

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

std::string topic_param, topic;
if (!private_nh.searchParam("footprint_topic", topic_param))
{
  topic_param = "footprint_topic";
}
private_nh.param(topic_param, topic, std::string("footprint"));
footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);

if (!private_nh.searchParam("published_footprint_topic", topic_param))
{
  topic_param = "published_footprint";
}
private_nh.param(topic_param, topic, std::string("oriented_footprint"));
footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("footprint", 1);

setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));

发布代价地图

创建地图的发布器实例,Costmap2DPublisher类是用于地图发布的封装类,在函数mapUpdateLoop中调用

1
2
3
4
5
6
7
8
9
10
11
12

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

//创建地图更新线程的控制量
stop_updates_ = false;
initialized_ = true;
stopped_ = false;

//创建一个timer去检测机器人是否在移动
robot_stopped_ = false;
//回调函数movementCB通过比较前后两个pose的差来判断机器人是否在移动
timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);

开启动态参数配置，它的回调函数Costmap2DROS::reconfigureCB会在节点启动时运行一次，加载.cfg文件的配置参数。这个函数给对应参数赋值，并创建了一个Costmap2DROS::mapUpdateLoop函数的线程，即地图更新线程。

1
2
3
4
5
6

//开启参数动态配置
  dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
  //回调函数reconfigureCB 除了对一些类成员的配置值做赋值以外，还会开启一个更新map的线程 
  dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,_2);
  dsrv_->setCallback(cb);
}

UpdateMap首先调用类内getRobotPose函数，通过tf转换，将机器人底盘系的坐标转换到global系，得到机器人的位姿。然后通过layered_costmap_调用LayeredCostmap类的updateMap函数，更新地图。

流程如下

整个costmap初始化，最关键和复杂的就是各层的加载和更新地图

Costmap2DROS, Costmap2D, LayeredCostmap三个类都属于命名空间costmap_2d，互相没有继承关系，而且都是独立的类

Costmap2D 类中实现了用来存储和访问2D代价地图的的基本数据结构. 它的定义在LayeredCostmap类的成员列表初始化里面，cell默认值default_value_的赋值在LayerdCostmap类的构造函数里。

地图图像原点在左下角，缺点是会增加理解难度，好处是在生成图像时，y可以 +1 增，符合真实世界中y向北表示增加。要画出图像时，须要上下颠倒，y往往要换成(map.info.height - y - 1)

map坐标系原点在该区域左下角坐标(origin_x_, origin_y_)附近。具体是在(origin_x_ + resolution/2, origin_x_ + resolution/2)

在一次导航过程中，world坐标系只有一个，原点固定。map坐标则随着区域出现而出现。由于一个栅格表示一个resolutin X resolution面积的真实区域，栅格坐标对应的world坐标是在该栅格的中心点坐标。

看全局代价地图的各个参数:

1
2
3
4
5
6
7
8
9

int nx = global_costmap->getSizeInCellsX();   //2200
int ny = global_costmap->getSizeInCellsY();	  //1600	
double global_resolution = global_costmap->getResolution();  // 0.025

double origin_x = global_costmap->getOriginX();    // -1
double origin_y = global_costmap->getOriginY();    // -34.6

double sizeMeterX = global_costmap->getSizeInMetersX();     // 109.975
double sizeMeterY = global_costmap->getSizeInMetersY();     //  79.975

global_resolution, origin_x, origin_y就是map.yaml中的resolution和origin参数

sizeMeterX 和 sizeMeterY 在rviz上用Measure工具测量一下地图的长宽即可得到

局部代价地图，yaml文件的参数是这样设置的:

1
2
3

width: 4.0
height: 4.0
resolution: 0.05

origin_x: 58, origin_y: 1.5 这里机器人的初值位置为(60, 3.5)，位于动态窗口的中心，计算就能得到这两个参数

sizeMeterX: 3.975000, sizeMeterY: 3.975000 其实就是4

这里会发现在全局代价地图的yaml里，没有设置width和height，因为它们是地图文件决定的，无需我们设置。而局部代价地图的动态窗口大小是我们决定的。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

// 就是为 costmap_ 分配内存
void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{
    boost::unique_lock<mutex_t> lock(*access_);
    delete[] costmap_;
    costmap_ = new unsigned char[size_x * size_y];
}

void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y, 
                          double resolution,
                          double origin_x, double origin_y)
{
    size_x_ = size_x;
    size_y_ = size_y;
    resolution_ = resolution;
    origin_x_ = origin_x;
    origin_y_ = origin_y;

    initMaps(size_x, size_y);
    // reset our maps to have no information
    resetMaps();
}

// 将 costmap_ 赋值为默认值
void Costmap2D::resetMaps()
{
    boost::unique_lock<mutex_t> lock(*access_);
    memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
}

void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{
    boost::unique_lock<mutex_t> lock(*(access_));
    unsigned int len = xn - x0;
    for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
      memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
}

1
2
3
4
5
6
7
8
9
10
11
12

// 根据像素坐标，获得 costmap_ 数组的索引
inline unsigned int getIndex(unsigned int mx, unsigned int my) const
{
    return my * size_x_ + mx;
}

// 根据 costmap_ 数组的索引，获得像素坐标
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
{
    my = index / size_x_;
    mx = index - (my * size_x_);
}

• void Costmap2D::deleteMaps(): 释放 costmap_

• bool saveMap(std::string file_name);

1
2
3
4
5
6

// 输入世界距离（世界坐标的距离，将其转换为单元格），输出均等距离
unsigned int Costmap2D::cellDistance(double world_dist)
{
  double cells_dist = max(0.0, ceil(world_dist / resolution_));
  return (unsigned int)cells_dist;
}

代价地图保存为pgm格式。全局代价地图保存是这样的

• void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const

1
2
3
4
5

void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
{
  wx = origin_x_ + (mx + 0.5) * resolution_;
  wy = origin_y_ + (my + 0.5) * resolution_;
}

把某个代价地图里的坐标转为世界坐标，常用于把机器人全局代价地图的坐标转为map坐标系的坐标。 在TebLocalPlannerROS::updateObstacleContainerWithCostmap()调用过: costmap_->mapToWorld(0, 0, wx, wy);

• void worldToMap(double robot_x, double robot_y, int mx, int my) const

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

// Convert from world coordinates to map coordinates
bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
{
    // 这里有时会出现，为什么 ???
    if (wx < origin_x_ || wy < origin_y_)
      return false;

    mx = (int) ((wx - origin_x_) / resolution_);
    my = (int) ((wy - origin_y_) / resolution_);
    // 出了代价地图的范围
    if (mx < size_x_ && my < size_y_)
      return true;

    return false;
}

把世界坐标转为某个代价地图里的坐标，最常用的是把机器人在map坐标系的坐标转为全局代价地图里的坐标，注意是转为像素坐标，不是base_link或常说的坐标系。 像素坐标系的原点是左下方的点 ，所以是跟size_x_和size_y_比较，这两个是地图的长宽，这样才能判断点是否出了代价地图的范围

1
2
3
4
5

tf::Stamped<tf::Pose> robot_pose;
costmap_ros_->getRobotPose(robot_pose);
robot_pose_ = PoseSE2(robot_pose);
unsigned int mx, my;
costmap_ros_->getCostmap()->worldToMap(robot_pose->x(), robot_pose->y(), mx, my);

1
2
3
4
5

if (!costmap_->worldToMap(x, y, mx, my)) {
    ROS_FATAL("The dimensions of costmap is too small to fully include robot's footprint");
    return;
}
unsigned char cost = costmap_->getCost(mx, my);

如果worldToMap返回false，那么是mx或my太大了，超出了代价地图的大小。如果不return，继续执行getCost会导致程序退出。还可以return -1，因为代价值是0~255。 有时返回false是因为 wx < originx || wy < originy 不明白原因

map坐标系坐标单元是栅格，只有整数才有意义。一般来说，只有在区域内的才有意义，即x范围是[0, size_x_-1]，y范围是[0, size_y_-1]。 这就涉及到worldToMapEnforceBounds 和 worldToMapNoBounds

• unsigned char getCost (unsigned int mx, unsigned int my) const    获得costmap中某个栅格的代价值。 mx, my是cell的坐标

• void setCost(unsigned int mx, unsigned int my, unsigned char cost)

设置代价地图中某个cell的代价值，在层的代码中使用很多

• void setDefaultValue(unsigned char c)

语句只有default_value_ = c;，调用的地方只有LayeredCostmap构造函数

1
2
3
4

if (track_unknown)  // 默认false
  costmap_.setDefaultValue(255);
else
  costmap_.setDefaultValue(0);

• unsigned char* getCharMap() const;     return A pointer to the underlying unsigned char array storing cost values     获取到这个地图中存储代价地图的指针，通过指针找到地图信息。主要在层的代码和ClearCostmapRecovery类中使用

1
2
3
4

unsigned char* Costmap2D::getCharMap() const
{
  return costmap_;
}

获得指定栅格周围的4个或8个栅格

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66

/**
 * @brief Determine 4-connected neighbourhood of an input cell, checking for map edges
 * @param idx input cell index
 * @param costmap Reference to map data
 * @return neighbour cell indexes
 */
std::vector<unsigned int> nhood4(unsigned int idx,
                                 const costmap_2d::Costmap2D& costmap)
{
  // get 4-connected neighbourhood indexes, check for edge of map
  std::vector<unsigned int> out;
  unsigned int size_x_ = costmap.getSizeInCellsX(),
               size_y_ = costmap.getSizeInCellsY();
  if (idx > size_x_ * size_y_ - 1)
  {
    ROS_WARN("Evaluating nhood for offmap point");
    return out;
  }
  if (idx % size_x_ > 0) {
    out.push_back(idx - 1);
  }
  if (idx % size_x_ < size_x_ - 1) {
    out.push_back(idx + 1);
  }
  if (idx >= size_x_) {
    out.push_back(idx - size_x_);
  }
  if (idx < size_x_ * (size_y_ - 1)) {
    out.push_back(idx + size_x_);
  }
  return out;
}

/**
 * @brief Determine 8-connected neighbourhood of an input cell, checking for map edges
 * @param idx input cell index
 * @param costmap Reference to map data
 * @return neighbour cell indexes
 */
std::vector<unsigned int> nhood8(unsigned int idx,
                                 const costmap_2d::Costmap2D& costmap)
{
  // get 8-connected neighbourhood indexes, check for edge of map
  std::vector<unsigned int> out = nhood4(idx, costmap);

  unsigned int size_x_ = costmap.getSizeInCellsX(),
               size_y_ = costmap.getSizeInCellsY();

  if (idx > size_x_ * size_y_ - 1) {
    return out;
  }

  if (idx % size_x_ > 0 && idx >= size_x_) {
    out.push_back(idx - 1 - size_x_);
  }
  if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) {
    out.push_back(idx - 1 + size_x_);
  }
  if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) {
    out.push_back(idx + 1 - size_x_);
  }
  if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) {
    out.push_back(idx + 1 + size_x_);
  }
  return out;
}

rqt_dep用于查看功能包的依赖关系图。

编译ROS时，有时会报错缺某个包，常常是 could not find *Config.cmake 或者 *-config.cmake，我以前一般的解决方法是：sudo apt-get install ros-kinetic-package_name，其实用rosdep更方便。

安装ROS时，会有一个命令 sudo apt-get install python-rosdep，在安装完成后，还会执行

1
2

sudo rosdep init
rosdep update

在src目录下运行rosdep check --from-path package_name，可以查找到所有依赖包。 安装所有依赖包的命令： rosdep install --from-path package_name

rosdep check --from-path src --ignore-src -r -y对整个工作空间检查依赖

最强大的就是rosdep install --from-path src --ignore-src -r -y，在工作空间路径执行，它magically安装了所有 package缺失的依赖项。

pair

map是将key和value放在一起来保存，pair是更一般化地将2个数据组合成一组数据，另外当一个函数需要返回2个数据的时候，可以选择pair。 pair的实现是一个struct，主要的两个成员变量是first 和 second，因为是使用struct不是class(默认访问权限public)，所以可以直接使用这两个成员变量。

头文件 #include <utility>， 类模板：template<class T1,class T2> struct pair。 T1和T2的类型可以不同

1
2
3
4
5
6
7

pair<T1, T2> p1;            //创建一个空的pair对象（使用默认构造），它的两个元素分别是T1和T2类型，采用值初始化。
pair<T1, T2> p1(v1, v2);    //创建一个pair对象，它的两个元素分别是T1和T2类型，其中first成员初始化为v1，second成员初始化为v2。
make_pair(v1, v2);          // 以v1和v2的值创建一个新的pair对象，其元素类型分别是v1和v2的类型。
p1 < p2;                    // 两个pair对象间的小于运算，其定义遵循字典次序：如 p1.first < p2.first 或者 !(p2.first < p1.first) && (p1.second < p2.second) 则返回true。
p1 == p2；                  // 如果两个对象的first和second依次相等，则这两个对象相等；该运算使用元素的==操作符。
p1.first;                   // 返回对象p1中名为first的公有数据成员
p1.second;                 // 返回对象p1中名为second的公有数据成员

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

pair<int ,double> p1;
p1.first = 1;
p1.second = 2.5;

pair<int, double> p2;
p2 = make_pair(1, 1.2);



std::pair<std::string, int> getPreson()
{
    return std::make_pair("Sven", 25);
}
 
std::string name;
int ages;
std::tie(name, ages) = getPreson();
std::cout << "name: " << name << ", ages: " << ages << std::endl;

string的find 函数

find()返回值是元素在string中的位置（下标记录），如果没有找到，那么会返回一个特别的标记npos，是一个很大很大的数。返回值可以看成是一个int型的数

1
2
3
4
5
6
7
8
9
10
11
12
13

string s("12345");
string::size_type  position;
// 返回34 在 s 中的下标位置
position = s.find("34");
//如果没找到，返回一个特别的标志c++中用npos表示，我这里npos取值是4294967295
if (position != s.npos) 
{
    printf("position is : %d\n" ,position);
}
else
{
    printf("Not found\n");
}