Intensity data PointCloud2

Hello!

Is there any way to publish ros2 message PointCloud2 with intensity data?

Hi @555kd - you can publish PointCloud2 messages with intensity data in Isaac Sim using the ROS2 bridge. The Isaac Sim provides a ROS2 bridge that allows you to publish and subscribe to ROS2 topics directly from the simulation environment.

Here is a general process:

  1. Create a Lidar sensor in Isaac Sim. This can be done using the omni.isaac.range_sensor extension. The Lidar sensor will generate point cloud data with intensity information.
  2. Set up a ROS2 bridge in Isaac Sim. This can be done using the omni.isaac.ros2_bridge extension. You will need to create a ROS2 Publish Point Cloud node in the Action Graph.
  3. Configure the ROS2 Publish Point Cloud node to publish the Lidar sensor data. You will need to set the sensor_prim_path property of the node to the path of the Lidar sensor in the stage.
  4. Start the simulation and the point cloud data with intensity information will be published to the specified ROS2 topic.

https://docs.omniverse.nvidia.com/isaacsim/latest/tutorial_ros2_rtx_lidar.html

https://docs.omniverse.nvidia.com/extensions/latest/ext_omnigraph/node-library/nodes/omni-isaac-ros2_bridge-humble/ros2publishpointcloud-1.html

Yes, I have already tried this.
ROS2 Publish Point Cloud node doesn’t publish intensity data.

I need a message contain XYZ and intensity fields.

This would be most likely resolved in the upcoming release in September

There’s still no way of publishing of intensity data using ROS2 Publish Point Cloud? I was also trying to make it work with Isaac Create Render Product + Isaac Compute RTX Lidar Point Cloud + ROS2 Publish Point Cloud, but with no sucess

Hello!
I did this by writing my own ros node that runs asynchronously in my standalone application, receives the “pointCloudData” and “intensity” attributes from Isaac’s “Compute RTX Lidar Point Cloud” node and then publishes PointCloud2 message.

1 Like

@555kd could you share an example of this?

1 Like

First of all you need to create ROS2 publisher:

import numpy as np

from omni.isaac.core.utils.prims import is_prim_path_valid
import omni.graph.core as og

from rclpy.node import Node

import sensor_msgs.msg as sensor_msgs
import std_msgs.msg as std_msgs
from builtin_interfaces.msg._time import Time

from astropy.coordinates import spherical_to_cartesian

PCL2_TOPIC_HZ = 10
RENDER_PATH = "/Render/PostProcess/SDGPipeline/"

class PCDPublisher(Node):

    def __init__(self):
        super().__init__('pcd_publisher_node')
        
        self.points = np.array([[1, 1, 1, 1]]) # [x,y,z,i]
        self.time = Time()

        self.pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, '/lidar_points', 10)
        timer_period = PCL2_TOPIC_HZ # 10 hz
        self.timer = self.create_timer(timer_period, self._lidar_callback)

    def _lidar_callback(self):
        if is_prim_path_valid(RENDER_PATH + "RenderProduct_Isaac_RtxSensorCpuIsaacComputeRTXLidarPointCloud"):
            lidar_compute_node_path = RENDER_PATH + "RenderProduct_Isaac_RtxSensorCpuIsaacComputeRTXLidarPointCloud"   
        for i in range(1,10):
            if is_prim_path_valid(RENDER_PATH + f"RenderProduct_Isaac_0{i}_RtxSensorCpuIsaacComputeRTXLidarPointCloud"):
                lidar_compute_node_path = RENDER_PATH + f"RenderProduct_Isaac_0{i}_RtxSensorCpuIsaacComputeRTXLidarPointCloud"

        # receiving lidar data from Isaac
        r_arr = og.Controller().node(lidar_compute_node_path).get_attribute("outputs:range").get()
        lat_arr = og.Controller().node(lidar_compute_node_path).get_attribute("outputs:elevation").get()
        lon_arr = og.Controller().node(lidar_compute_node_path).get_attribute("outputs:azimuth").get()

        x_arr, y_arr, z_arr = spherical_to_cartesian(r_arr, lat_arr, lon_arr)
        cartesian_arr = np.column_stack((x_arr.value, y_arr.value * (-1), z_arr.value))

        intensity_from_isaac = og.Controller().node(lidar_compute_node_path).get_attribute("outputs:intensity").get()

        # adding intensity in [x,y,z]                                                                                                            
        self.points = np.column_stack((cartesian_arr, intensity_from_isaac))

        ros_time = self.get_clock().now().to_msg()
        self.time = ros_time
        self.pcd = self._create_point_cloud(self.points, self.time, 'base_scan')
        self.pcd_publisher.publish(self.pcd)
        
    def _create_point_cloud(self, points, time, parent_frame):
        """ Creates a point cloud message.
        Args:
            points: Nx4 array of xyz positions and intensity data.
            parent_frame: frame in which the point cloud is defined
        Returns:
            sensor_msgs/PointCloud2 message
        """
        
        ros_dtype = sensor_msgs.PointField.FLOAT32
        dtype = np.float32
        itemsize = np.dtype(dtype).itemsize  # A 32-bit float takes 4 bytes.

        data = points.astype(dtype).tobytes()
        fields = [
                sensor_msgs.PointField(name='x', offset=0, datatype=ros_dtype, count=1),
                sensor_msgs.PointField(name='y', offset=4, datatype=ros_dtype, count=1),
                sensor_msgs.PointField(name='z', offset=8, datatype=ros_dtype, count=1),
                sensor_msgs.PointField(name='intensity', offset=12, datatype=ros_dtype, count=1),
            ]
        header = std_msgs.Header(stamp=time, frame_id=parent_frame)

        return sensor_msgs.PointCloud2(
            header=header,
            height=1,
            width=points.shape[0],
            is_dense=True,
            is_bigendian=False,
            fields=fields,
            point_step=(itemsize * 4),  # Every point consists of four float32s.
            row_step=(itemsize * 4 * points.shape[0]),
            data=data
        )

Then you need to create async task for compute node due Isaac Sim is running

async def my_task_lidar(): 
    publisher = PCDPublisher()
    
    while rclpy.ok():   
        rclpy.spin_once(publisher)
        
        await asyncio.sleep(0.05)
    publisher.unregister()
    publisher = None

Finally you need to start node:

frame = 0 
while simulation_app.is_running():

    if frame==48:
        asyncio.ensure_future(my_task_lidar())
     
    simulation_context.step(render=True)
    frame = frame + 1

simulation_context.stop()
simulation_app.close()
1 Like

@555kd thanks so much for the response, in the end I did something similar but it was slow and hard to get the intensities to match the real sensor. I’m not sure what your application is but I just set the intensity values to zero in my training data and used only xyz for each point. This means it matches the real world data much more easily and also it did not affect the precision of my detector.

1 Like