【ROS】从numpy点云文件向moveit添加点云场景

实验室项目要求将点云输入到moveit中作为障碍，记录一下将numpy点云文件添加到moveit中作为场景的方法。

本文参考：link

1. 点云输入到moveit场景中

机器人环境一般用八叉树来表示，OctoMap已经是Moveit的一个插件，叫做Occupany Map Updater插件，能够从不同的传感器源生成八叉树，目前有两种更新器：
PointCloud Occupancy Map Updater: This plugin can take input in the form of point clouds (sensor_msgs/PointCloud2)
Depth Image Occupancy Map Updater: This plugin can take input in the form of input depth images (sensor_msgs/Image)

这里我们的输入为点云文件，故采用第一种八叉树更新器，方法如下

（1）建立sensor_moveit_config包。放在universal_robot文件夹下作为新的功能包，方便各机械臂调用

catkin_create_pkg sensor_moveit_config roscpp rospy std_msgs moveit_ros_move_group moveit_ros_visualization

（2）在sensor_moveit_config包内新建config和launch文件。
在config文件夹内新建sensor_rgbd.yaml配置文件

# /universal_robot/sensor_moveit_config/config/sensor_rgbd.yaml
sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
  point_cloud_topic: /point_cloud
  max_range: 10
  padding_offset: 0.01
  padding_scale: 1.0
  point_subsample: 1
  filtered_cloud_topic: output_cloud

在launch文件夹内新建kinect2_moveit_sensor_manager.launch.xml和sensor_manager.launch.xml

# /universal_robot/sensor_moveit_config/launch/kinect2_moveit_sensor_manager.launch.xml
<launch>
   <rosparam command = "load" file="$(find sensor_moveit_config)/config/sensor_rgbd.yaml" />
</launch>

# /universal_robot/sensor_moveit_config/launch/sensor_manager.launch.xml
<launch>

  <!-- This file makes it easy to include the settings for sensor managers -->  

  <!-- Params for the octomap monitor -->
  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
  <param name="octomap_resolution" type="double" value="0.025" />
  <param name="max_range" type="double" value="5.0" />

  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
  <arg name="moveit_sensor_manager" default="kinect2" />
  <include file="$(find sensor_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
</launch>

切换到catkin_ws编译

catkin_make

在{想要使用的机械臂}_moveit_config/launch/move_group.launch文件中添加如下代码

<include ns="move_group" file="$(find sensor_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
   <arg name="moveit_sensor_manager" value="kinect2" />
</include>

到这里我们就完成了将点云转换为八叉树输入到moveit的过程，其中move_group节点通过订阅/universal_robot/sensor_moveit_config/config/sensor_rgbd.yaml文件中的point_cloud_topic话题获取点云信息，该点云信息的消息结构为sensor_msgs/PointCloud2。

2. Numpy点云数据转换到sensor_msgs/PointCloud2格式

首先，看一下sensor_msgs/PointCloud2消息的定义

$ rosmsg info sensor_msgs/PointCloud2
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id # 坐标系id
uint32 height # 点云行数
uint32 width # 一行点云总个数
sensor_msgs/PointField[] fields
  uint8 INT8=1
  uint8 UINT8=2
  uint8 INT16=3
  uint8 UINT16=4
  uint8 INT32=5
  uint8 UINT32=6
  uint8 FLOAT32=7
  uint8 FLOAT64=8
  string name
  uint32 offset
  uint8 datatype
  uint32 count
bool is_bigendian #储存方式
uint32 point_step # 储存一个点云所需字节长度，单位是byte
uint32 row_step # 储存一行点云所需字节长度 row_step = point_step * width
uint8[] data # 点云数据
bool is_dense

知道了消息的定义后，就可以写一个publisher将numpy点云数据转化为PointCloud2并发布到上一节中订阅的点云话题中（我们使用的是/point_cloud），publisher代码如下

#!/usr/bin/env python
# /universal_robot/ur5_moveit_config/script/talker.py

import rospy
from std_msgs.msg import String
from std_msgs.msg import Header
import numpy as np

from sensor_msgs.msg import PointField
from sensor_msgs.msg import PointCloud2

def talker(pc_path):
    pub = rospy.Publisher('/point_cloud', PointCloud2, queue_size=10)
    rospy.init_node('pc_publisher', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        pc = np.load(pc_path) # 输入的点云尺寸为480x640x3
        points = pc.reshape(-1,3)
        
        # PointCloud2消息定义
        msg = PointCloud2()      
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "world" # 坐标系id，可以在RViz中找到
        msg.height = 1
        msg.width = points.shape[0]
        msg.fields = [
            PointField('x', 0, PointField.FLOAT32, 1),
            PointField('y', 4, PointField.FLOAT32, 1),
            PointField('z', 8, PointField.FLOAT32, 1)
            ]
        msg.is_bigendian = False
        msg.point_step = 12
        msg.row_step = msg.point_step * points.shape[0]
		msg.is_dense = False
        msg.data = points.astype(np.float32).tobytes()

        pub.publish(msg)
        print('send')
        rate.sleep()

if __name__ == '__main__':
    pc_path = '/root/catkin_ws/src/universal_robot/ur5_moveit_config/test_pc.npy'
    talker(pc_path)
    

完成后在命令行按顺序调用：

roslaunch ur_gazebo ur5_joint_limited.launch
roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch sim:=true limited:=true
roslaunch ur5_moveit_config moveit_rviz.launch config:=true
rosrun ur5_moveit_config talker.py

效果图