为MoveIt添加基于3D LiDAR的环境感知

---

前言

MoveIt 的环境感知，可以通过接入 3D 的 LiDAR 或者深度相为机械臂构建基于“Octomap”的周围环境。MoveIt 通过“Occupancy Map Updater”插件处理 3D 数据进行环境感知。

官网有基础的教程链接，但内容细节描述得不够清晰，本文基于官网链接的过程，使用 Robosense 的 16 线 3D LiDAR 做完整实现的说明，同时澄清一些官网未提及的细节。

---

1. 设置传感器属性参数

moveit_config 在通过 Setup Assistant 创建时无论是否设置 3D sensor 都会自动创建 3D sensor 的参数文件，命名为“sensors_3d.yaml”。首先，需要将 3D sensor 的参数添加到该文件。以 Robosense 的 16 线 3D LiDAR 为示例，在文件“sensors_3d.yaml”中添加如下内容：

# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it
sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: rslidar_points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud
    #ns: Kinect

本文示意的是3D LiDAR的属性参数，深度相机的属性参数参考官网说明链接

需要重点关注的参数：

名称	说明
point_cloud_topic	指定 3D LiDAR 所发布类型为“sensor_msgs/PointCloud2”消息的主题，本文示例为 Robosense 的 16 线激光，其发布主题为“rslidar_points”
ns	命名空间，当有多个 LiDAR 时可以通过其进行区分，由于只用到了一个 LiDAR，这里暂时注释

---

2. 更新launch文件

3D sensor 的属性参数是由 launch 文件调用进入 ROS param server的，因此，需要将 3D sensor 的属性参数文件更新到 launch 文件中。3D sensor 的属性参数文件“sensors_3d.yaml”在 moveit_config 中的 launch 调用关系为：

更新 launch 文件“sensor_manager.launch.xml”，为其加入如下内容：

需要重点关注的参数：

名称	说明
octomap_frame	3D sensor 所发布消息的坐标系 id，可以通过 rostopic 查看 3D sensor 的消息确定

该参数可以通过查看其发布的消息来确定，以 Robosense 的 16 线激光雷达为例，首先将其运行，在终端输入命令：

roslaunch rslidar_sdk start.launch

随后，通过 rostopic 确定其发布消息的主题，在终端输入命令：

rostopic list

确定其发布消息主题为“/rslidar_points”，再次通过 rostopic 确定其坐标系 id，继续在终端输入命令：

rostopic echo /rslidar_points/header/frame_id

由此，可以得到其消息的坐标系 id 为“/rslidar”

---

3. 运行验证

随后，运行验证 3D sensor 信息是否能被 MoveIt 加载，首先运行 3D sensor，依然以 Robosense 的 16 线激光雷达为例，在终端输入命令：

roslaunch rslidar_sdk start.launch

随后运行 moveit_config 的 demo.launch，以苇航智能的 whi_moveit_config 为例，在新终端输入命令：

roslaunch whi_moveit_config demo.launch

本文示例以作者公司苇航智能的 moveit_config 为示例，读者可以使用 panda_moveit_config

遗憾的是，运行后不能看到类似官网链接中的 Octomap 环境：

可以分析 3D sensor 发布的点云信息带有坐标，这些坐标如何和机械臂所在坐标系进行关联，目前为止，我们还未提供这种关联关系。因此初步分析问题应该是由于 TF tree 不完整所导致。可以通过 rqt_graph 查看当前的 TF tree 留存与官网示例的 TF tree 进行对比进行分析问题所在，在新终端输入命令：

rosrun rqt_graph rqt_graph

随后，运行官网示例，在终端输入命令：

roslaunch moveit_tutorials obstacle_avoidance_demo.launch

运行成功后，在另一个终端输入命令：

rosrun rqt_graph rqt_graph

对比两个TF关系，可以看到我们缺失了“to_panda”到“move_group”与“to_camer”到“move_group”的两条静态 TF 的发布路径，通常静态发布路径都是由 launch 文件发布，因此查看官网示例的 launch 文件“obstable_avoidance_demo.lanch”，可以看到其有两个“static_transform_publisher”：

参照该文件将上述两条“static_transform_publisher”加入到 3D sensor 的运行 launch 文件中：

由于使用苇航智能的 moveit_config 和 Robosense 的 16 线 LiDAR，所以，第一个 TF transform，终点修改为了“whi_link0” - whi_config的固定 link 坐标系 id，第二个 TF transform 的起点修改为了“rslidar” - Robosense LiDAR 的坐标系 id

另一个需要关注的是，实际情况中需要依据 3D sensor 与机械臂的实际位置关系，修改 TF transform “to_camera”的坐标转换数值，本章节示例的是其间没有位置偏差，都是“0”

最后，再次运行查看，依然在两个终端分别输入命令，运行 3D sensor：

roslaunch rslidar_sdk start.launch

另一个终端运行moveit_config:

roslaunch whi_moveit_config demo.launch

此时，能看到来自于 3D sensor 的环境信息，同时再次通过 rqt_graph 查看 TF tree，可以看到与官网的关系保持一致：

---

可能碰到的问题

运行时提示“Transform error of sensor data: Invalid argument “/rslidar” passed to lookupTransform argument source_frame in tf2 frame_ids can not start with a ‘/’ like: ;”：

提示已经很明显，frame_id 不能以字符‘/’开头，查看 Robosense LiDAR 的 config 文件“config.yaml”，其给出的 frame_id 为“/rslidar”，将字符‘/’删除后解决问题

---

总结

3D 环境感知对机械臂有很广泛的运用需求，本文通过实现官网教程的过程，澄清一些官网教程中未能说明的细节，希望通过本文说明能够有类似需求的读者提供一个参考。

如果文中出现了描述错误以及不清晰的地方，欢迎指正，共同交流：[email protected]