/open3d_ros_pointcloud_conversion

2 Python API functions for point cloud conversion between Open3D and ROS. Compatible for XYZ and XYZRGB point type.

Primary LanguageCMakeMIT LicenseMIT

Content: Two functions for Open3D and ROS point cloud conversion

Supported cloud type: XYZ and XYZRGB

I didn't see any good Python function for converting point cloud datatype between Open3D and ROS, so I made this repo.

The script lib_cloud_conversion_between_Open3D_and_ROS.py contains 2 functions:

  • convertCloudFromOpen3dToRos
  • convertCloudFromRosToOpen3d

where the ROS cloud format is indicating this: "sensor_msgs/PointCloud2.msg".

The script also contains a test case, which does such a thing:
(1) Read a open3d_cloud from .pcd file by Open3D. (2) Convert it to ros_cloud. (3) Publish ros_cloud to topic. (4) Subscribe the ros_cloud from the same topic. (5) Convert ros_cloud back to open3d_cloud. (6) Display it and save it to .pcd file.

You can test it by:

$ rosrun open3d_ros_pointcloud_conversion lib_cloud_conversion_between_Open3D_and_ROS.py

References

Some explanation and testing of sensor_msgs::PointCloud2's contents

  1. To test the format of PointCloud2 before writing these python conversion functions, I used the following c++ pcl functions to generate a PointCloud2 message from pcl's cloud.
  pcl::PointCloud<PointXYZRGB>::Ptr pcl_cloud = (some function to read in a cloud);
  sensor_msgs::PointCloud2 ros_cloud;
  pcl::toROSMsg(*pcl_cloud, ros_cloud);
  1. Publish ros_cloud, and view it by "rostopic echo", we get the following contents (see below).
  2. We can see the message has:
    a) 32 byte length,
    b) "rgb" at 16th byte,
    c) "rgb" datatype 7, which is float (4 bytes).
  3. Despite the above format converted from pcl::toROSMsg, we can still set things like this (And this is what this script is using):
    a) 16 byte length,
    b) "rgb" at 12th byte,
    c) "rgb" datatype 6, which is uint32.

----- Contents of PointCloud2 converted from pcl::toROSMsg and viewed by "rostopic echo" -----

header: 
  seq: 16
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: ""
height: 1
width: 1706
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
  - 
    name: "rgb"
    offset: 16
    datatype: 7
    count: 1
is_bigendian: False
point_step: 32
row_step: 54592
data: ...