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
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
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
