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类型消息