Error in laser_scan_matcher, creating a correct pointcloud?

asked 2018-12-15 12:59:30 -0500

bierschi gravatar image

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()
edit retag flag offensive close merge delete

Comments

Did you find a solution? I am using velodyne_driver package for VLP16 and having the same exact troubles

Robb gravatar image Robb  ( 2019-12-17 12:34:41 -0500 )edit