0
$\begingroup$

I have 2 setups: Setup #1: Ubuntu 20.04 focal Virtual cloud machine with arm64 architecture with ROS1 noetic, classic gazebo (which runs husky simulation from clearpath robotics)

Setup #2: Ubuntu 22.04 Jammy personal laptop with amd64 architecture with ROS2 humble hawksbill, ign gazebo fortress (which runs sample lidar visualization as example)

I am trying to segment the point cloud's ground and non-ground points. I wrote two scripts (one each for ROS1 and ROS2) to extract xyz co-ordinates from the topic (which contains VLP-16 lidar data) But before that, I wanted to verify whether the ros_numpy's pointcloud2_to_xyz_array method is doing the math correctly by visualizing the XYZ co-ordinates using matplotlib and comparing with the actual point cloud visualization shown by rviz/ rviz2. For simplicity, robots are not moving and are static.

In Setup#1 (ROS1): Husky robot is spawned in an empty gazebo world. points topic contains the message typesensor_msgs/PointCloud2. rviz shows the ground points as circular rings which is expected but the XYZ plot shows some random points in the middle which I did not expect.

ROS1 single message echoed by the topic: points

header: 
  seq: 5908
  stamp: 
    secs: 651
    nsecs:  33000000
  frame_id: "velodyne"
height: 1
width: 15000
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: "intensity"
    offset: 12
    datatype: 7
    count: 1
  - 
    name: "ring"
    offset: 16
    datatype: 4
    count: 1
  - 
    name: "time"
    offset: 18
    datatype: 7
    count: 1
is_bigendian: False
point_step: 22
row_step: 330000
data: a lot of numbers over here
is_dense: True

ROS1 (matplotlib on left containing many xyz points which I don't comprehend), RVIZ visualization which is correct

In Setup#2 (ROS2): Example robot is spawned in a children's playground. point_cloud2 topic contains the message type sensor_msgs/PointCloud2. Both rviz2 and the XYZ plot using matplotlib shows the ground points as circular rings which is expected.

ROS2 single message echoed by the topic: point_cloud2

header:
  stamp:
    sec: 918
    nanosec: 800000000
  frame_id: vehicle_blue/lidar_link/gpu_lidar
height: 16
width: 1875
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: intensity
  offset: 16
  datatype: 7
  count: 1
- name: ring
  offset: 24
  datatype: 4
  count: 1
is_bigendian: false
point_step: 32
row_step: 60000
data: a lot of numbers
is_dense: false

ROS2 is fine

I am not able to understand from where did the extra points come from in Setup #1?

Script used for ROS1/ Setup # 1

#!/usr/bin/env python
import rospy

from sensor_msgs.msg import PointCloud2
import numpy as np
import ros_numpy as rnp
import sensor_msgs
import math
from mpl_toolkits import mplot3d
import time


import matplotlib.pyplot as plt

class pointcloud_subscriber:
    def __init__(self):

        rospy.init_node('listener', anonymous=True)

        print("Entered class")
        self.fig = plt.figure()
        self.ax = plt.axes(projection='3d')
        self.ax.set_xlabel('X axis')
        self.ax.set_ylabel('Y axis')
        self.ax.set_zlabel('Z axis')
        

        rospy.Subscriber("points", PointCloud2, self.listener_callback)
        plt.show()
        rospy.spin()



    def listener_callback(self, msg):
        print("Listening START")
        #rospy.loginfo(rospy.get_caller_id() + "I heard %s", msg.data)
        # msg.__class = PointCloud2
        # offset_sorted = {f.offset: f for f in msg.fields}
        # msg.fields = [f for(_, f) in sorted(offset_sorted.items())]
        pc_np = rnp.point_cloud2.pointcloud2_to_xyz_array(msg,remove_nans=True)
        distance_array = np.zeros(shape=(pc_np.shape[0],1))
        for i in range(pc_np.shape[0]): 
            distance = math.sqrt(pc_np[i,0]**2 + pc_np[i,1]**2 + pc_np[i,2]**2)
            #print(distance)
            distance_array[i,0] = distance
        print(np.amin(distance_array))
        print("Minimum distance index is: " + str(np.argmin(distance_array)))
        mean = np.mean(pc_np[:,2])
        sd = np.std(pc_np[:,2])
        data_ground = pc_np[(pc_np[:,2] < mean + 1.5*sd) & (pc_np[:,2] > mean - 1.5*sd)]
        data_wo_ground = pc_np[(pc_np[:,2] > mean + 1.5*sd) | (pc_np[:,2] < mean - 1.5*sd)]
        print(data_ground.shape, data_wo_ground.shape)
        
        j=0
        while j <= 10: 
            self.ax.scatter(pc_np[:,0], pc_np[:,1], pc_np[:,2])
        # time.sleep(20)
        

        #print(distance_array.shape)
        #print(pc_np[1,0])
        print("Listening END. Size is: " + str(pc_np.shape))


def main(args=None):
    points_filteration = pointcloud_subscriber()


if __name__ == '__main__':
    main()

The only difference which I found between lidar message in ROS1 vs ROS2 is the height which is 1 in ROS1 and 16 in ROS2. I don't think it should make any difference in xyz point extraction. Please provide your viewpoints as well.

Script used for ROS2/ Setup #2
    
    import rclpy
    from rclpy.node import Node
    
    from sensor_msgs.msg import PointCloud2
    import numpy as np
    import point_cloud2 as rnp
    import sensor_msgs
    import math
    from mpl_toolkits import mplot3d
    
    
    import matplotlib.pyplot as plt
    
    
    class MinimalSubscriber(Node):
    
        def __init__(self):
    
            # print("Entered main function")
            # self.fig = plt.figure()
            # self.ax = plt.axes(projection='3d')
            # self.ax.set_xlabel('X axis')
            # self.ax.set_ylabel('Y axis')
            # self.ax.set_zlabel('Z axis')
    
            super().__init__('minimal_subscriber')
            self.subscription = self.create_subscription(
                PointCloud2,
                '/point_cloud2',
                self.listener_callback,
                10)
            self.subscription  # prevent unused variable warning
    
        def listener_callback(self, msg):
            # self.get_logger().info('I heard: "%s"' % msg.data)
            #print("Listening START")
            #msg.__class = PointCloud2
            #offset_sorted = {f.offset: f for f in msg.fields}
            #msg.fields = [f for(_, f) in sorted(offset_sorted.items())]
            pc_np = rnp.pointcloud2_to_xyz_array(msg,remove_nans=True)
            distance_array = np.zeros(shape=(pc_np.shape[0],1))
            for i in range(pc_np.shape[0]): 
                distance = math.sqrt(pc_np[i,0]**2 + pc_np[i,1]**2 + pc_np[i,2]**2)
                #print(distance)
                distance_array[i,0] = distance
            print(np.amin(distance_array))
            print("Minimum distance index is: " + str(np.argmin(distance_array)))
            mean = np.mean(pc_np[:,2])
            sd = np.std(pc_np[:,2])
            data_ground = pc_np[(pc_np[:,2] < mean + 1.5*sd) & (pc_np[:,2] > mean - 1.5*sd)]
            data_wo_ground = pc_np[(pc_np[:,2] > mean + 1.5*sd) & (pc_np[:,2] < mean - 1.5*sd)]
            print(data_ground.shape, data_wo_ground.shape)
    
            # self.ax.scatter(pc_np[:,0], pc_np[:,1], pc_np[:,2])
            # plt.show()
            
            #print(distance_array.shape)
            #print(pc_np[1,0])
            print("Listening END. Size is: " + str(pc_np.shape))
    
    
    def main(args=None):
        rclpy.init(args=args)
        minimal_subscriber = MinimalSubscriber()
    
        rclpy.spin(minimal_subscriber)
    
        # Destroy the node explicitly
        # (optional - otherwise it will be done automatically
        # when the garbage collector destroys the node object)
        minimal_subscriber.destroy_node()
        rclpy.shutdown()
    
    
    if __name__ == '__main__':
        main()

Please help.

[UPDATE]: I tried to simulate Setup #2 with no environment and found that the XYZ plot also shows the bad plot (lots of extra points in the center) just like Setup #1. Upon closer inspection, I feel it is because of the scale of the z-axis. For e.g. In Setup #1 bad figure, Z-axis scale ranges from 1.0350 metres to 1.0550 metres or 103.5 cm to 105.5 cm which means that it varies by 2 cm only. If I magnify the plot, visualization becomes even worse. Although, in an ideal world, all the lidar rings should be perfectly horizontal but it seems that simulation is not giving such results which are also not visualized in rviz by naked eyes. So, pointcloud2_to_xyz_array seems to be working fine. I hope that it is a convincing answer to my own question and for everyone who is reading it. Please feel free to add your viewpoints.

Thanks, Dev

$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.