先提一下栅格地图的占用值更新的方式。当雷达扫到一个障碍时，会对击中点的栅格更新一次占用，这时在ROS地图下这个点将被表示为障碍物．

如果ROS中设置障碍物的occupied_thresh是0.65，free_thresh是0.25。第一次更新地图时，这个格子的占用值为 $1/1 = 1 > 0.65$，就会将ROS地图中这个格子设置为100，表示障碍物．如果这个障碍离开了这个位置，在之后的过程中，如果有4次雷达扫描这个栅格都是空闲状态，也就是没有障碍物，则这个格子的占用值将变为 $1/5 = 0.2 <0.25$ ，所以就会将ROS地图中对应的栅格设置为0，变成可通过区域了．

这就导致了不能通过一次两次的更新之后的值，来代表这个栅格是占用还是空闲的，因为这个状态十分有可能是错的．只有通过长时间的累计之后，最后通过这个值是否大于零，来近似地将这个栅格设置为是否是障碍物，这时候依然不能百分百地确定这个栅格是否就是障碍物．

在上一篇的UpdateBounds函数最后到了

1
2
3
4

for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
  raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
}

清理传感器到障碍物间的cell，会首先处理测量值越界的问题，然后调用

1
2
3
4

MarkCell marker(costmap_, FREE_SPACE);
// and finally... we can execute our trace to clear obstacles along that line
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);

1
2
3
4
5
6
7

double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;

......省略

unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
touch(px, py, min_x, min_y, max_x, max_y);

1
2
3
4
5
6
7
8
9
10

/*
 * @brief  Given distance in the world, convert it to cells
 * @param  world_dist: The world distance
 * @return The equivalent cell distance
 */
unsigned int Costmap2D::cellDistance(double world_dist)
{
  double cells_dist = max(0.0, ceil(world_dist / resolution_));
  return (unsigned int)cells_dist;
}

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

class MarkCell
{
public:
  MarkCell(unsigned char* costmap, unsigned char value) :
      costmap_(costmap), value_(value)
  {
  }
  inline void operator()(unsigned int offset)
  {
    costmap_[offset] = value_;
  }
private:
  unsigned char* costmap_;
  unsigned char value_;
};

updateCosts

代价地图中每个cell可用255个不同值中任何一个值，可是下层数据结构仅需要3个值。具体来说，每个cell仅需要3个值来表示cell的3种状态：free，occupied，unknown。 当投影到代价地图时候，每种状态被赋一个特定的代价值，也就是说 每个cell的cost值是由这个cell对应的各层中对应的cell的状态进行加权得到的。 如果列有一定量的占用就被赋代价值。 如果存储的障碍物信息是3D的，需要将每一列的障碍物信息投影成2D后才能放入到代价地图。

更新障碍地图代价，将机器人足迹范围内设置为 FREE_SPACE，并且在 bound 范围内将本层障碍地图的内容合并到主地图上。

障碍物层是将传感器检测到的点云投影到地图中，不同的传感器来源会分别存到Observation类中。只要遍历存储Observation的容器，将检测到的点云的每个点分别投影到地图中，将对应的网格的代价值设为LETHAL_OBSTACLE，实现函数在ObstacleLayer::updateBounds中：

清除代价地图的方式也很好理解，以单线激光雷达的激光线为例子，当激光线发射出去之后，在某处检测到一个障碍物，那说明：从发射的地方至某处之间是free的，那么这之间的旧的障碍物应当被删除，这之间网格的代价值应当被修改为free

可能是Bresenham2D的round off error(舍入误差)造成的. It marks a particular cell as obstacle but the next time when the sensor reading changes instantaneously, it no longer traces it through the same path and hence, the blob seem remains in the costmap.

added 2 lines of code to the Bresenham2D algorithm. This basically clears the cell to the left and right of the grid through which the Line segment constructed by the algorithm passes. This results in loosing some resolution of the map, but the solution works pretty well in real life application

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

// A 2D implementation of Bresenham's raytracing algorithm, applies an action 
// at each step template<class ActionType>
inline void bresenham2D( ActionType at, unsigned int abs_da, unsigned int abs_db, 
                        int error_b, int offset_a, int offset_b, unsigned int offset, 
                        unsigned int max_length)
{
  unsigned int end = std::min(max_length, abs_da);
  for (unsigned int i = 0; i < end; ++i)
  {
    at(offset);
    at(offset+1); // **ADDED THIS LINE**
    at(offset-1); // **ADDED THIS LINE**
    offset += offset_a;
    error_b += abs_db;
    if ((unsigned int)error_b >= abs_da)
    {
      offset += offset_b;
      error_b -= abs_da;
    }
  }
  at(offset);
}

python2.7和python3的冲突问题

之前在ROS安装和编译等常见问题.md提到了python3.8和python2.7冲突的问题，那次是不同文件可以分别用不同版本的python运行，但是如果同一个文件里用到了python的两个版本，那就不好解决了。

比如一个文件需要用到python3的函数，然后发现import tf报错，没法用tf了，因为tf2_ros是针对python2编译的，为了适应python3 (melodic)，进行如下步骤：

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

sudo apt update
sudo apt install python3-catkin-pkg-modules python3-rospkg-modules python3-empy
Prepare catkin workspace

mkdir -p ~/catkin_ws/src; cd ~/catkin_ws
catkin_make
source devel/setup.bash
wstool init
wstool set -y src/geometry2 --git https://github.com/ros/geometry2 -v 0.6.5
wstool up
rosdep install --from-paths src --ignore-src -y -r
Finally compile for Python 3

catkin_make --cmake-args \
            -DCMAKE_BUILD_TYPE=Release \
            -DPYTHON_EXECUTABLE=/usr/bin/python3 \
            -DPYTHON_INCLUDE_DIR=/usr/include/python3.6m \
            -DPYTHON_LIBRARY=/usr/lib/x86_64-linux-gnu/libpython3.6m.so
source devel/setup.bash

让python支持中文注释

在源文件的初始部分，而且必须放在第一行，添加#coding=utf-8 或者 #coding=gbk 或# -- coding: gb2312 --

参考：
ImportError: dynamic module does not define module export function (PyInit__tf2)

所用全局路径的参数

1
2
3
4
5
6

planner_window_x: 0.0
planner_window_y: 0.0
default_tolerance: 0.0

publish_scale: 100
planner_costmap_publish_frequency: 0.0

看代码GlobalPlanner::makePlan中的if (found_legal)部分，再看GlobalPlanner::getPlanFromPotential函数，继续定位到path_maker_->getPath。这里就需要看path_maker是什么东西，在文件中发现

1
2
3
4
5
6

if (use_grid_path)   // 参数
    //栅格路径，从终点开始找上下或左右4个中最小的栅格直到起点
    path_maker_ = new GridPath(p_calc_);
else
    //梯度路径，从周围八个栅格中找到下降梯度最大的点
path_maker_ = new GradientPath(p_calc_);

将if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) 中的0.5改为1

查来查去，终于发现其实就是全局代价地图里，本来就处于障碍的附近，所以规划如此奇怪。只是换到了室外环境，不习惯看全局代价地图了

多传感器数据融合的时候，由于各个传感器采集数据的频率的不同，例如odom 50Hz、Imu 100Hz、camera 25Hz，需要将传感器数据进行时间同步后才能进行融合。

分别订阅不同的需要融合的传感器的主题，通过TimeSynchronizer统一接收多个主题，只有在所有的topic都有相同的时间戳时，才会产生一个同步结果的回调函数，在回调函数里处理同步时间后的数据。

注意: 只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了，则整个回调函数永远无法触发回调。 频率一般趋于和最低的频率一样。 当多个主题频率一致的时候，回调函数的频率(融合后的频率)可能会小于订阅主题的频率。

message_filters::Synchronizer省去了不同线程(回调函数)之间的同步问题，两线程的数据直接放到一个回调里处理。 但是它对传感器数据进行时间同步后，回调函数里的两个话题的时间戳会完全一样，纳秒级的相同。这是经过了ROS的调整，但有时候可能不是我们想要的。

对齐传感信息时间戳有两种方式, 一种是时间戳完全对齐 ExactTime Policy, 另一种是时间戳相近 ApproximateTime Policy, 前者更为严格, 但有时会没有结果而无法进行回调函数。所以一般还是用后者

message_filters
message_filters::Synchronizer的使用
传感器数据之间的时间同步问题

octomap_server除了将点云地图转化为基于Octree的OctoMap，还能将点云地图转化为二维地图。可以增量建3D的 OctoMaps，还提供了能保存地图的节点octomap_saver. octomap_server节点很消耗内存，增量建图的过程中内存持续上升，因为当中不存在内存释放，没有静态储存八叉树图信息

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

<node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <!-- resolution in meters per pixel -->
    <param name="resolution" value="0.05"/>
    <!-- name of the fixed frame, needs to be "/map" for SLAM -->
    <param name="frame_id" type="string" value="$(arg frame_id)"/>
    <param name="sensor_model/max_range" value="150.0"/>
    <param name="latch" value="true"/>
    <!-- 需要和 publish_pointcloud 的frame_id相同，否则会导致Octomap没有发布数据 -->
    <!-- topic from where pointcloud2 messages are subscribed，改为自己的点云话题 -->
    <remap from="/cloud_in" to="$(arg cloud_topic)"/>
    <remap from="/projected_map" to="$(arg map_topic)"/>
</node>

也就是将点云数据发布到一个指定的 topic 上，然后调用 Octomap 在ROS下的srv组件进行实时转换，并发布到另外一个 Octomap topic 上去，最后通过rviz 进行显示Octomap。注意octomap的输入话题（topic）和数据的坐标系(frame_id)两个参数的设置， 通常octomap 没有数据输出都是由于这两个参数设置错误导致的。

Rviz配置

安装octomap-rviz-plugins后，打开rviz,这时候点开Add选项，会多一个octomap_rviz_plugins模组。 其中的OccupancyGrid是显示三维概率地图，也就是octomap地图。OccupancyMap是显示二维占据栅格地图。

打开rviz，在里面添加OccupancyGrid，OccupancyMap，Map显示选项,显示话题选择octomap_full或者octomap_binary

octomap topics are 3D occupancy maps, which would be used by some other 3D planning approach (e.g., move_it). They are not the same as /grid_map or /proj_map from rtabmap_ros. For 3D trajectories (having to move in Z, e.g., quadcopter), then using OctoMap with OMPL/move_it can be another approach, though not sure if there is one most popular approach for the 3D case.

参考：
Octomap在ROS环境下实时显示，但点云来源是ORB稠密点云

在调试ROS程序时，经常需要运行多个节点程序，如果每个都打开一个终端然后输入指令非常繁琐，可以写一个脚本文件，每次运行这个脚本就能一次运行多个节点，方便高效。其实就是利用ubuntu的terminator：

1

sudo apt-get -y install terminator

打开config文件，在terminal3下面，给command选项起个名字，例如command = COMMAND1。其它的terminal同理，后面的脚本会把这个名字改成运行的指令。

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
67
68

[global_config]
  enabled_plugins = CustomCommandsMenu, ActivityWatch, LaunchpadCodeURLHandler, APTURLHandler, LaunchpadBugURLHandler
  suppress_multiple_term_dialog = True
[keybindings]
[layouts]
  [[default]]
    [[[child0]]]
      fullscreen = False
      last_active_term = 6465ad81-9563-4994-a9ea-4c73e642264a
      last_active_window = True
      maximised = True
      order = 0
      parent = ""
      position = 91:27
      size = 1823, 861
      title = q@ubuntu: ~
      type = Window
    [[[child1]]]
      order = 0
      parent = child0
      position = 908
      ratio = 0.499725726824
      type = HPaned
    [[[child2]]]
      order = 0
      parent = child1
      position = 427
      ratio = 0.499419279907
      type = VPaned
    [[[child5]]]
      order = 1
      parent = child1
      position = 427
      ratio = 0.499419279907
      type = VPaned
    [[[terminal3]]]
      command = COMMAND1
      order = 0
      parent = child2
      profile = default
      type = Terminal
      uuid = 6465ad81-9563-4994-a9ea-4c73e642264a
    [[[terminal4]]]
      command = COMMAND2
      order = 1
      parent = child2
      profile = default
      type = Terminal
      uuid = 40bece0b-c0b1-4510-b0d4-555a76b0df61
    [[[terminal6]]]
      command = COMMAND3
      order = 0
      parent = child5
      profile = default
      type = Terminal
      uuid = 54ebe730-2576-4963-a96f-bf5d63167da9
    [[[terminal7]]]
      command = COMMAND4
      order = 1
      parent = child5
      profile = default
      type = Terminal
      uuid = 2941b7ee-3345-4062-bfc3-cd68f3a636d4
[plugins]
[profiles]
  [[default]]
    background_image = None
    scrollback_lines = 5000

下面制作自己的脚本script.sh文件，内容如下：

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

#!/bin/bash
 
## change these to whatever you actually need
command1="cd ~/TEB;source devel/setup.bash;roslaunch teb_local_planner_tutorials robot_carlike_in_stage.launch; bash"
command2="sleep 2s;roslaunch yocs_velocity_smoother standalone.launch; bash"
command3="sleep 3s;source ~/control/devel/setup.bash; roslaunch control control.launch; bash"
command4="sleep 4s;source ~/ReedsShepp/devel/setup.bash; roslaunch steering_functions steering_functions.launch; bash"
 
## Modify terminator's config
sed -i.bak "s#COMMAND1#$command1#; s#COMMAND2#$command2#; s#COMMAND3#$command3#; s#COMMAND4#$command4#;" ~/.config/terminator/config
 
## Launch a terminator instance using the new layout
terminator -l default
 
## Return the original config file
mv ~/.config/terminator/config.bak ~/.config/terminator/config

如果脚本启动不了terminator了，那就检查一下~/.config/terminator路径下的配置文件config，把它改回上面的代码即可。

过程参考两篇博客
多机通信 1
多机通信 2
需要注意的地方

• /etc/hosts里的是IP和计算机名，不是用户名

• 环境变量ROS_MASTER_URI里最好用计算机名。也可以自己设置名称。不过这样如果其他人不知道，还按计算机名设置，就会失败。

• 所有设置完成后，重启主机的ROS节点

但是我设置的多机通信失败，因为工控机有3个IP。与只有1个IP的电脑是正常通信的。

速度

• min_vel_theta 设置一个相对较小的值就可以了，一般0.1、0.2都可以，默认0.4会导致最小速度过大，无法准确到达目标点。
• max_trans_vel: 0.25   平移速度最大值绝对值，默认0.55
• min_trans_vel: 0.1 默认0.1，一个较小的最小平移速度是必须的，它会影响低速行驶的状态，尤其是到达目标点时，最小速度过大会导致越过目标点，始终无法到达，可以在0-0.1之间寻找合适的大小。
• max_vel_x: 0.25   x方向最大速度的绝对值
• min_vel_x 注意是否要有负值，也就是是否允许倒车，实际中过高的倒车速度是应当避免的。 如果后方没有传感器就应该禁止倒车。 机器人在180度左右的转向，且两侧障碍物状况类似时，会出现左右摆动无法选择方向的问题，应该是受全局路径影响了，偶然情况下我设置了0.05的后退速度，发现每次转弯会先往后退一点，然后转向也变得很好了
• max_vel_y: 0.0   y方向最大速度的绝对值.
• min_vel_y: 0.0   y方向最小速度的绝对值.
• max_rot_vel: 0.25   最大旋转速度的绝对值.

• min_rot_vel: 0.1   最小旋转速度的绝对值.

• acc_lim_x: 0.1   x方向的加速度绝对值，单位m/sec^2 ，默认2.5

• acc_lim_y: 0.0   y方向的加速度绝对值,该值只有全向移动的机器人才需配置.
• acc_lim_th: 0.1   旋转加速度的绝对值.单位是radians/sec^2

注意加速度不能太小，否则导航时会特别慢，看上去是机器人扭来扭去地走向记录点。

Goal Tolerance Parameters

• yaw_goal_tolerance: 0.2   到达目标点时偏行角允许的误差，单位弧度.
• xy_goal_tolerance: 0.22   到达目标点时,在xy方向与目标点的距离误差.
• latch_xy_goal_tolerance: false   若设置为true,如果到达容错距离内,机器人就会原地旋转,即使转动会跑出容错距离外.

前向仿真参数

• sim_time: 1.7   给定轨迹上的点之间的间隔尺寸，单位为 meters (double, default: 1.7)

• sim_granularity: 0.025    (double, default: 0.025)

• vx_samples: 3   样本数 (integer, default: 3)

• vy_samples: 10   (integer, default: 10)

• vth_samples: 20   角速度的样本数 (integer, default: 20)

• controller_frequency: 20.0   调用该控制器的频率 (double, default: 20.0)

Trajectory Scoring Parameters 路径评分参数

• path_distance_bias: 32.0   控制器靠近给定路径的权重 (double, default: 32.0)

• goal_distance_bias: 24.0    控制器尝试达到其本地目标and控制着速度的权重。(double, default: 24.0)

• occdist_scale: 0.01   控制器尝试避免障碍物的权重 (double, default: 0.01)

• forward_point_distance: 0.325   从机器人中心点到另一个得分点的距离 (double, default: 0.325)

• stop_time_buffer: 0.2   机器人在碰撞前必须停止的时间 (double, default: 0.2)

• scaling_speed: 0.25   缩放机器人足迹的速度的绝对值，单位为m/s (double, default: 0.25)

• max_scaling_factor: 0.2   缩放机器人足迹的最大因子 (double, default: 0.2)

• publish_cost_grid: false    (bool, default: false) 是否发布规划器在规划路径时使用的代价网格(cost grid). 如果设置为true,那么就会在~/cost_cloud话题上发布sensor_msgs/PointCloud2类型消息. Each point cloud represents the cost grid and has a field for each individual scoring function component as well as the overall cost for each cell, taking the scoring parameters into account.

衡量每次导航轨迹的公式如下：

1
2
3
4

cost =
  path_distance_bias * (distance to path from the endpoint of the trajectory in meters)
  + goal_distance_bias * (distance to local goal from the endpoint of the trajectory in meters)
  + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254) )

Oscillation Prevention Parameters

• oscillation_reset_dist: 0.05   机器人必须运动多少米远后才能复位震荡标记 (double, default: 0.05)

Debugging

• publish_traj_pc : true   将规划的轨迹在RVIZ上进行可视化
• prune_plan: false    机器人前进时，是否清除身后1m外的轨迹.默认是true
• publish_cost_grid_pc: true   将代价值进行可视化显示 (bool, default: false)，是否发布规划器在规划路径时的代价网格.如果设置为true,那么就会在~/cost_cloud话题上发布sensor_msgs/PointCloud2类型消息