1. 
   这是常见的一种情况

1. 起初经过观察，发现在一些地方容易出现，而且是都在雷达的左侧出现，于是怀疑是雷达自身问题，最后把angle_min的绝对值改小，不再出现这个问题。
   雷达扫描到不存在的障碍

后来又出现这种现象，而且不分左右，有时看不出在哪边

1
2
3
4
5
6

planner_->visualize();
visualization_->publishObstacles(obstacles_);
// 很简单，显示 via_point, 坐标来源是 via_points_
visualization_->publishViaPoints(via_points_);
// 很简单，实际调用的是 base_local_planner::publishPlan
visualization_->publishGlobalPlan(global_plan_);

TebOptimalPlanner::visualize

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

void TebOptimalPlanner::visualize()
{
  if (!visualization_)
    return;
 
  visualization_->publishLocalPlanAndPoses(teb_);
  if (teb_.sizePoses() > 0)
      visualization_->publishRobotFootprintModel(teb_.Pose(0), *robot_model_);
  
  if (cfg_->trajectory.publish_feedback)
      visualization_->publishFeedbackMessage(*this, *obstacles_);
}

最后部分是发布feedback消息 (single trajectory overload)，话题teb_feedback. 需要设置publish_feedback为true，默认为false

feedback message包括规划的轨迹，速度和时间信息(temporal information)，也包括 active obstacles

publishLocalPlanAndPoses很简单，需要注意的是

1
2
3
4

// 不明白z为什么这样赋值 ???
pose.pose.position.z = cfg_->hcp.visualize_with_time_as_z_axis_scale *
    teb.getSumOfTimeDiffsUpToIdx(i);
// getSumOfTimeDiffsUpToIdx 就是从起点到当前位姿i的所有deltaT的总和

publishRobotFootprintModel也很简单，机器人在局部路径第一个点的时候，可视化轮廓，就是一些对visualization_msgs::Marker的操作。不过对不同的轮廓类型，调用的函数不同

publishFeedbackMessage

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

void TebVisualization::publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles)
{
  FeedbackMsg msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = cfg_->map_frame;
  msg.selected_trajectory_idx = 0;
  msg.trajectories.resize(1);
  msg.trajectories.front().header = msg.header;

  teb_planner.getFullTrajectory(msg.trajectories.front().trajectory);
  // add obstacles
  msg.obstacles_msg.obstacles.resize(obstacles.size());
  for (std::size_t i=0; i<obstacles.size(); ++i)
  {
    msg.obstacles_msg.header = msg.header;
    // copy polygon
    msg.obstacles_msg.obstacles[i].header = msg.header;
    obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
    // copy id
    msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet

    // orientation
    //msg.obstacles_msg.obstacles[i].orientation =; // TODO
    // copy velocities
    obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
  }
  feedback_pub_.publish(msg);
}

publishObstacles

这里需要注意的是点障碍的可视化

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

visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = "PointObstacles";     marker.id = 0;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);
    
for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
{
  boost::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(*obst);      
  if (!pobst) 		continue;

  if (cfg_->hcp.visualize_with_time_as_z_axis_scale < 0.001)
  {
    geometry_msgs::Point point;
    point.x = pobst->x();
    point.y = pobst->y();
    point.z = 0;
    marker.points.push_back(point);
  }
  else // Spatiotemporally point obstacles become a line
  {
    marker.type = visualization_msgs::Marker::LINE_LIST;
    geometry_msgs::Point start;
    start.x = pobst->x();   start.y = pobst->y();
    start.z = 0;
    marker.points.push_back(start);

    geometry_msgs::Point end;
    double t = 20;
    Eigen::Vector2d pred;
    pobst->predictCentroidConstantVelocity(t, pred);
    end.x = pred[0];
    end.y = pred[1];
    end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale * t;
    marker.points.push_back(end);
  }
}
// 省略 marker的着色
teb_marker_pub_.publish( marker );

HomotopyClassPlanner::visualize

visualization_的赋值在HomotopyClassPlanner::initialize 最后的 setVisualization(visual);

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

if (visualization_)
{
  		// Visualize graph
  	if (cfg_->hcp.visualize_hc_graph && graph_search_)
    		visualization_->publishGraph(graph_search_->graph_);

   // Visualize active tebs as marker
   visualization_->publishTebContainer(tebs_);

   // Visualize best teb and feedback message if desired
   TebOptimalPlannerConstPtr best_teb = bestTeb();
   if (best_teb)
   {
      // 只发布best_teb的局部路径，类型是nav_msgs::Path 和 geometry_msgs::PoseArray
   	// 对应话题 local_plan 和 teb_poses
     // 位姿点的z = cfg_->hcp.visualize_with_time_as_z_axis_scale * 
   	// teb.getSumOfTimeDiffsUpToIdx(i)

     visualization_->publishLocalPlanAndPoses(best_teb->teb() );
     if (best_teb->teb().sizePoses() > 0)   // 路径的位姿点个数
         visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *robot_model_);

     if (cfg_->trajectory.publish_feedback)  // feedback message
     {
       int best_idx = bestTebIdx();
       if (best_idx>=0)
           visualization_->publishFeedbackMessage(tebs_, 
                (unsigned int) best_idx,  *obstacles_  );
     }
   }
}
else ROS_DEBUG("Ignoring HomotopyClassPlanner::visualize() call, since no visualization class was instantiated before.");

消息类型

sensor_msgs/PointCloud2类型如下:

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

# N维的点云，点的数据以二进制blob形式保存，其layout用fields数组描述

# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.

Header header

点云的2D结构，如果点云无序，height是1，width就是点云的length
uint32 height
uint32 width

# 用二进制数据blob的形式，描述channels 及其layout
PointField[] fields

bool    is_bigendian
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[]   data       # 点的数据，size = row_step * height
bool is_dense        # True if there are no invalid points

sensor_msgs/PointCloud类型如下:

1
2
3
4
5
6
7
8
9

Header header

# 在header坐标系中的3D点云
geometry_msgs/Point32[]  points

# Each channel should have the same number of elements as points array,
# and the data in each channel should correspond 1:1 with each point.
# Channel names in common practice are listed in ChannelFloat32.msg.
ChannelFloat32[] channels

从sensor_msgs::PointCloud2类型转为sensor_msgs::PointCloud，而且获知坐标

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

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

sensor_msgs::PointCloud2 in_pcl;
sensor_msgs::PointCloud out_pcl;
sensor_msgs::convertPointCloud2ToPointCloud(in_pcl, out_pcl);
for (int i=0; i < out_pcl.points.size(); i++)
{
	ROS_INFO("out_pcl x: %f, y: %f, z: %f", 
          out_pcl.points[i].x, out_pcl.points[i].y, out_pcl.points[i].z);
}

消息的发布

意外发现ROS可以直接发布pcl的类型，不必转为ROS的消息类型

1
2

typedef pcl::PointCloud<PointT> PointCloudT;
PCL2_publisher_ = nh.advertise<PointCloudT>("pointcloud2", 1, this);

几种点云类型的转换

点云格式主要有四种，sensor_msgs::PointCloud，也就是点云1已经弃用，主要用点云2，但是点云1有时还有用。比如点云2转为点云1消息，可看到包含多少个点，rostopic echo --noarr pcl1/points，结果可以是<array type: geometry_msgs/Point32, length: 256000>，可看到每个点的坐标值。

• sensor_msgs::PointCloud

• sensor_msgs::PointCloud2

• pcl::PCLPointCloud2 — PCL data structure mostly for compatibility with ROS (I think)

• pcl::PointCloud — standard PCL data structure

find_package里添加sensor_msgs, pcl_ros, pcl_conversions

ROS PointCloud2 和 PointCloud的转换

1
2

sensor_msgs::PointCloud out_pcl;
sensor_msgs::convertPointCloud2ToPointCloud(*pcl_dist_msg, out_pcl);

反过来就用sensor_msgs::convertPointCloudToPointCloud2

可以使用point_cloud_converter在点云1和点云2 之间转换

1
2

PointCloudConverter initialized to transform from PointCloud (/points_in) to PointCloud2 (/points2_out).
PointCloudConverter initialized to transform from PointCloud2 (/camera_/depth/points) to PointCloud (/pcl)

PointCloud2 转 pcl::PCLPointCloud2

1
2
3
4

sensor_msgs::PointCloud2 cloud;
pcl::PCLPointCloud2  pcl_pc2;

pcl_conversions::toPCL(cloud, pcl_pc2

pcl::PCLPointCloud2 转 pcl::PointCloud < pcl::PointXYZ >

1
2
3
4

pcl::PCLPointCloud2  pcl_pc2;
pcl::PointCloud < pcl::PointXYZ >  pcl_cloud;

pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);

pcl::PointCloud < pcl::PointXYZ > 转 sensor_msgs::PointCloud2

1
2
3
4
5

#include <pcl_conversions/pcl_conversions.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr inCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(*inCloud, cloud);

反过来就是fromROSMsg(const sensor_msgs::PointCloud2 &, pcl::PointCloud<T>&)

include

参考: pcl_conversions

InflationLayer的构造及初始化方法和StaticLayer类似。该层是基于已有地图进行膨胀，因此不需要有缺省值。由Layer类单独派生出InflationLayer类. 用于执行每个cell的障碍物膨胀，由于它的父类中不含Costmap2D类，所以其实膨胀层自身没有栅格地图要维护，这一点和另外两层有区别。

• inflation_radius (double, default: 0.55) 障碍物在地图中向外扩展的膨胀区的半径
• cost_scaling_factor (double, default: 10.0) 膨胀过程中应用到代价值的比例因子。这个值越大，代价衰减越严重

膨胀层所用的参数：inflation params

可以修改膨胀层代码 inflation_layer.h， 添加函数double getInflationRadius() const 和 double getInscribedRadius() const

每个周期传感器数据进来后，都要在代价地图底层占用结构上执行标记和清除障碍操作，并且这种结构会被投影到代价地图附上相应代价值。 这完成之后，对代价赋值为costmap_2d::LETHAL_OBSTACLE的每个cell执行障碍物的膨胀操作，即从每个代价cell向外传播代价值，直到用户定义的膨胀半径为止。这里确实只需要对状态为LETHAL_OBSTACLE的cell进行膨胀操作即可。

ldd

命令的作用是打印共享库的依赖关系

1. ldd不是一个可执行程序，而只是一个shell脚本。ldd能够显示可执行模块的 dependency，其原理是通过设置一系列的环境变量，如下：LD_TRACE_LOADED_OBJECTS、LD_WARN、LD_BIND_NOW、LD_LIBRARY_VERSION、 LD_VERBOSE等。

2. ldd的工作原理，其实质是通过ld-linux.so（elf动态库的装载器）来实现的。ld-linux.so模块会先于executable模块程序工作，并获得控制权，因此当上述的那些环境变量被设置时，ld-linux.so选择了显示可执行模块的dependency。
   实际上可以直接执行ld-linux.so模块，如：/lib/ld-linux.so.2 --list program（这相当于ldd program）

1
2

/usr/bin/ld: skipping incompatible /home/user/lib/libtinyalsa.so when searching for -ltinyalsa
/usr/bin/ld: cannot find -ltinyalsa: No such file or directory

so库的架构不匹配，可以通过file name.so查看so库的架构，也可以使用readelf命令查看so文件架构信息的示例: readelf -h libexample.so。输出so文件的头部信息，其中包括Machine字段，即架构信息。Machine字段的值对应于不同的架构，比如x86、aarch64、MIPS等。

编译时常常会出错: undefine reference XXX，显然是不识别函数，比如 _ZN9fast_gicp7NDTCudaIN3pcl9PointXYZIES2_EC1Ev，但是这个名称看得不清晰，看不出原来的函数原型。

先执行nm -D libhdl_localization_nodelet.so | grep NDTCuda | grep EC1Ev，输出

1

U  _ZN9fast_gicp7NDTCudaIN3pcl9PointXYZIES2_EC1Ev

再执行命令 c++filt _ZN9fast_gicp7NDTCudaIN3pcl9PointXYZIES2_EC1Ev (去掉前面的 U)， 输出函数原型

1

fast_gicp::NDTCuda<pcl::PointXYZI, pcl::PointXYZI>::NDTCuda()

ldconfig

有时装完某个库后，需要使用命令sudo ldconfig -v，否则在程序运行后会出现找不到动态库的问题: error while loading shared libraries: liblog4cpp.so.5: cannot open shared object file: No such file or directory

ldconfig是一个动态链接库管理命令，为了让动态链接库为系统所共享,还需运行动态链接库的管理命令

ldconfig通常在系统启动时运行,而当用户安装了一个新的动态链接库时,就需要手工运行这个命令.主要是在默认搜寻目录(/lib和/usr/lib)以及动态库配置文件/etc/ld.so.conf内所列的目录下，搜索出可共享的动态链接库(格式如lib*.so*)，进而创建出动态装入程序(ld.so)所需的连接和缓存文件.

ldconfig与运行程序时有关，跟编译时一点关系都没有。 不管做了什么动态库的变动后，最好都ldconfig一下

问题 error while loading shared libraries … file too short

这个错误应该是从别处拷贝so文件过来时出现了软链接错误。删除，重新创建这个软连接即可。
比如A,B,C的链接关系: A --> B, B --> C。先把C重命名，删掉A,B。重新建立软链接

1
2

ln -s C B
ln -s B A

• base_global_planner: 指定全局规划器

• base_local_planner: 指定局部规划器

• recovery_behaviors: true 是否允许恢复行为，恢复行为为原地旋转。 有时候不规则的机器人做恢复行为是比较危险的，容易发生碰撞，圆形机器人不会有这个问题

• shutdown_costmaps: false 当 move_base进入inactive状态时候，决定是否停用节点的costmap (bool, default: false)

• controller_frequency: 20.0 控制器频率，即向基座发送速度命令的频率

• planner_patience: 5.0 在空间清理操作执行前，路径规划器等待多长时间（秒）用来找出一个有效规划 (double, default: 5.0)

• controller_patience: 20.0 在空间清理操作执行前，控制器会等待多长时间（秒）用来找出一个有效控制 (double, default: 15.0)

• planner_frequency: 5.0 全局路径规划的更新速率，可以是0. 当机器在避障表现较好时，此参数较小即可。 如果local planner问题很大，再频繁更新全局路径，可能会离原先的最优路径越来越远，直到陷入局部障碍物

• oscillation_timeout: 10.0 执行修复操作之前，允许的震荡时间是几秒

• oscillation_distance: 0.5 机器人需要移动多少距离才算作没有震荡

• conservative_reset_dist: 3.0 当在地图中清理出空间时候，距离机器人几米远的障碍将会从costmap清除。 实验中发现局部地图经常会有移动障碍物遗留，并且不会去除，只有在移动后才能清除，部分只能在恢复行为后清除，所以有一个插件是专门用来做local障碍物清除的
  剩余参数用于无法行动之后的恢复行为，震荡表示机器人在很短的距离反复运动，可以设置距离、时间来判断是否为震荡以及多久后执行恢复操作

move_base有很多unknown type的订阅

1
2
3
4
5

* /footprint [unknown type]
* /footprint_radius [unknown type]
* /move_base/cancel [unknown type]
* /move_base/goal [unknown type]
* /move_base_simple/goal [unknown type]

move_base源码没有获取机器人当前位姿的地方，这属于局部路径算法的范畴。只能获得全局目标的位姿

bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, vector& plan)

此函数只在一个地方被调用,就是planThread()线程被唤醒之后开始规划路径

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

boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex() ));
plan.clear();
if(planner_costmap_ros_ == NULL) {
    log.error("Planner costmap ROS is NULL, unable to create global plan");
    return false;
}
//get the starting pose of the robot
tf::Stamped<tf::Pose> global_pose;
if(!planner_costmap_ros_->getRobotPose(global_pose)) {
    log.warn("Unable to get starting pose of robot, unable to create global plan");
    return false;
}
geometry_msgs::PoseStamped start;
tf::poseStampedTFToMsg(global_pose, start);

// 这里就是 GlobalPlanner::makePlan 了
if(!planner_->makePlan(start, goal, plan) || plan.empty()){
    ROS_WARN("[move_base] Failed to find a  plan to point (%.2f, %.2f)", 
    	goal.pose.position.x, goal.pose.position.y);
    return false;
}
return true;

这里要参考全局路径规划(二) makePlan

MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)

req参数包含了起点和目标信息，这个服务回调函数的的核心是planner_->makePlan

1. move_base server必须在inactive状态，否则不向下执行

2. 判断global planner的costmap是否存在;若req给定机器人初始位姿则使用,否则使用getRobotPose获得机器人所在位置做初始位姿

3. 调用clearCostmapWindows完成对机器人区域的clear， clear_radius由参数设置

4. 调用 if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()) 这是完成plan计算的核心部分。判断这个调用是否成功，如果失败，则在目标区域附近搜索，多次更改req.goal的值，并重新调用makePlan;新的goal如下图:
   

各个可能位置之间的水平和竖直间距和req.tolerance正相关

如果还是失败，则此次路径规划无解。如果成功,直接break循环,把原来无法达到的目标点插入global_plan容器的最后,local planner一般会从新的目标点导航到原目标点.

1. 将global_plan拷贝到resp.plan

MoveBase::clearCostmapWindows(double size_x, double size_y)

planService·用到了clearCostmapWindows函数，它只被调用了这一次，过程如下：

• 通过planner_costmap_ros_->getRobotPose(global_pose); 获取在全局地图的global_pose

• 以这个点为中心，找到以size_x和size_y为边长的矩形的四个顶点

• 调用planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE);完成对机器人所在区域的clear工作

• 以上同样的操作在controller_costmap_ros上也操作一遍，这样globa costmap 和local costmap都已经在机器人所在的区域完成clear工作

障碍层地图通过订阅传感器话题，将传感器输出的障碍物信息存进buffer（剔除过高、过远的点），在本层地图上将观测到的点云标记为障碍物，将传感器到点云的连线上的点标记为FREE_SPACE。最后在bound范围内，将本层地图合并到主地图上。

配置参数

通用代价地图中的障碍层常常是这样设置的:

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

obstacles:
 enabled: true
 obstacle_range: 2.0
 raytrace_range: 5.0
 track_unknown_space: true
 combination_method: 1
 observation_sources: laser_scan_sensor pointcloud2_sensor
 laser_scan_sensor:
    data_type: LaserScan
    topic: /scan
    marking: true
    clearing: true
    max_obstacle_height: 1.0
    min_obstacle_height: -1.0

 pointcloud2_sensor:
    topic:          /people_cloud
    data_type:      PointCloud2
    sensor_frame:   "/base_link"
    obstacle_range: 5.0
    raytrace_range: 5.0
    observation_persistence: 5.0
    marking:        true
    clearing:       true
    min_obstacle_height: 0.0
    max_obstacle_height: 2.0

• observation_persistence: 2.0. 如果设置为0，代价地图只考虑最近的观测数据

• min_obstacle_height: 传感器读数的最小高度（以米为单位）视为有效。通常设置为地面高度。决定是否将地面的部分加入到代价地图，默认0

车周围实际没有障碍物，不同参数的情况如下：

这种情况下，避障太保守了。

• max_obstacle_height: 默认2，单位米。插入代价地图的障碍物的最大高度，应当比机器人高度稍微高一点，应该是用于有机械臂的情况。设置为大于全局max_obstacle_height参数的值将会失效，设置为小于全局max_obstacle_height的值将过滤掉传感器上大于该高度以的点。

• obstacle_range: 设置机器人检测障碍物的最大范围，意思是说超过该范围的障碍物，并不进行检测，只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物。对能否及早发现障碍物至关重要 ，跟车速也有关系。 如果车速0.6，obstacle_range设置为1.5，连正常避障都实现不了，至少也得3

• raytrace_range: 在机器人移动过程中，实时清除代价地图上的障碍物的最大范围，更新可自由移动的空间数据。假如设置该值为3米，那么就意味着在3米内的障碍物，本来开始时是有的，但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以自由移动的空间

增大raytrace_range， obstacle_range会增大数据量，影响代价地图刷新

• inf_is_valid: 默认是false，但是对点云障碍无效，仅对雷达scan有效:

1
2
3
4

if (inf_is_valid)
{
  ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
}

障碍层所用的参数：obstacles params

不能将近处障碍添加到代价地图

调来调去，发现min_obstacle_height还是要设置为-0.3比较合适，这时因为地面的原因，导致局部代价地图的四角都有了障碍，因此再降低obstacle_range避免这一现象

局部代价地图中无法清除不在视野的障碍

costmap raytraces to clear out obstacles. 障碍物要想从地图上清除，它所占据的空间必须被新的观测看到。但是参数observation_persistence常常无效。

last_updated_ is updated on every new cloud that needs to be added to the buffer. purgeStaleObservations函数 checks the cloud’s time stamp with last_updated_. When we stop publishing a point cloud last_updated_ will never be updated. Hence the observations will not time-out and remain in the observation_list_. This may result in the following warning, when the robot drives away from the clouds in observation_list_: Sensor origin at (x, y) is out of map bounds. The costmap cannot raytrace for it

将文件observation_buffer.cpp中的函数purgeStaleObservations中的last_updated_改为nh_->now()，会影响共享库liblayers.so 和 libcostmap_2d.so

1
2

nav2_util::LifecycleNode::SharedPtr  nh
last_updated_(nh->now())