Error in laser_scan_matcher, creating a correct pointcloud?
Currently, i´m trying to use the laser_scan_matcher with a pointcloud created from velodyne hdl-32E laser. But if i execute the laser_scan_matcher, i got this error messages:
Error in scan matching
:err: Strange FOV: -0.026826 rad = -1.537021 deg
While i´ve tried to solve the problem, i found a similar question:
https://answers.ros.org/question/2995...
So my question is, how can i ensure that my created Pointcloud has the correct field of view? The Pointcloud looks fine in rviz
I create the pointcloud from the nclt dataset ( http://robots.engin.umich.edu/nclt/ )
every help is highly appreciated!
read from binary file:
def read_next_velodyne_packet(self):
try:
magic = self.f_bin_velodyne.read(8)
if magic == '': # EOF reached
return -1, None
if not self.verify_magic(magic):
print "Could not verify magic"
return -1, None
num_hits = struct.unpack('<I', self.f_bin_velodyne.read(4))[0]
utime = struct.unpack('<Q', self.f_bin_velodyne.read(8))[0]
self.f_bin_velodyne.read(4) # padding
data = []
for i in range(num_hits):
x = struct.unpack('<H', self.f_bin_velodyne.read(2))[0]
y = struct.unpack('<H', self.f_bin_velodyne.read(2))[0]
z = struct.unpack('<H', self.f_bin_velodyne.read(2))[0]
i = struct.unpack('B', self.f_bin_velodyne.read(1))[0]
l = struct.unpack('B', self.f_bin_velodyne.read(1))[0]
x, y, z = self.convert_velodyne(x, y, z)
data += [x, y, z, float(i), float(l)]
return utime, data
except Exception as e:
print(e)
convert the packets to pointcloud messages:
def xyz_array_to_pointcloud2(self, utime, points):
timestamp = rospy.Time.from_sec(utime / 1e6)
msg = PointCloud2()
msg.header.stamp = timestamp
msg.header.frame_id = self.json_configs['frame_ids']['velodyne_lidar']
num_values = points.shape[0]
assert(num_values > 0)
NUM_FIELDS = 5
assert(np.mod(num_values, NUM_FIELDS) == 0)
num_points = num_values / NUM_FIELDS
assert(len(points.shape) == 1)
msg.height = 1
FLOAT_SIZE_BYTES = 4
msg.width = num_values * FLOAT_SIZE_BYTES
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('i', 12, PointField.FLOAT32, 1),
PointField('l', 16, PointField.FLOAT32, 1)
]
msg.is_bigendian = False
msg.point_step = NUM_FIELDS * FLOAT_SIZE_BYTES
msg.row_step = msg.point_step * num_points
msg.is_dense = False
msg.width = num_points
msg.data = np.asarray(points, np.float32).tostring()
return timestamp, msg
and finally the call of this 2 methods:
timestamp, pc2 = self.laser_data.xyz_array_to_pointcloud2(utime_vel, np.array(dat_vel))
self.bag.write('velodyne', pc2, t=timestamp)
utime_vel, dat_vel = self.laser_data.read_next_velodyne_packet()
Did you find a solution? I am using velodyne_driver package for VLP16 and having the same exact troubles