def write_lidar(self, folder, topic, frame, ext): print(' Writing lidar: ' + topic) records = self.read_csv(self.path + self.name + '/' + folder + '/' + 'timestamps.txt') seq = 0 for row in tqdm(records): filename = row[0] + ext timestamp = row[1] scan = np.fromfile(self.path + self.name + '/' + folder + '/' + filename, dtype=np.float32) scan = scan.reshape(-1, 4) header = Header() header.frame_id = frame #header.stamp = self.timestamp_to_stamp(timestamp) header.stamp.secs = seq / 10 header.stamp.nsecs = (seq % 10) * 100000000 seq += 1 fields = [ pc2.PointField('x', 0, pc2.PointField.FLOAT32, 1), pc2.PointField('y', 4, pc2.PointField.FLOAT32, 1), pc2.PointField('z', 8, pc2.PointField.FLOAT32, 1), pc2.PointField('i', 12, pc2.PointField.FLOAT32, 1) ] pc_message = pc2.create_cloud(header, fields, scan) self.bag.write(topic, pc_message, t=pc_message.header.stamp)
def _make_point_field(num_field): msg_pf1 = pc2.PointField() msg_pf1.name = np.str('x') msg_pf1.offset = np.uint32(0) msg_pf1.datatype = np.uint8(7) msg_pf1.count = np.uint32(1) msg_pf2 = pc2.PointField() msg_pf2.name = np.str('y') msg_pf2.offset = np.uint32(4) msg_pf2.datatype = np.uint8(7) msg_pf2.count = np.uint32(1) msg_pf3 = pc2.PointField() msg_pf3.name = np.str('z') msg_pf3.offset = np.uint32(8) msg_pf3.datatype = np.uint8(7) msg_pf3.count = np.uint32(1) msg_pf4 = pc2.PointField() msg_pf4.name = np.str('intensity') msg_pf4.offset = np.uint32(16) msg_pf4.datatype = np.uint8(7) msg_pf4.count = np.uint32(1) if num_field == 4: return [msg_pf1, msg_pf2, msg_pf3, msg_pf4] msg_pf5 = pc2.PointField() msg_pf5.name = np.str('label') msg_pf5.offset = np.uint32(20) msg_pf5.datatype = np.uint8(4) msg_pf5.count = np.uint32(1) return [msg_pf1, msg_pf2, msg_pf3, msg_pf4, msg_pf5]
def define_fields(self): self.fields = list() field = pcl2.PointField() field.name = "x" field.offset = 0 field.datatype = pcl2.PointField.FLOAT32 field.count = 1 self.fields.append(field) field = pcl2.PointField() field.name = "y" field.offset = 4 field.datatype = pcl2.PointField.FLOAT32 field.count = 1 self.fields.append(field) field = pcl2.PointField() field.name = "z" field.offset = 8 field.datatype = pcl2.PointField.FLOAT32 field.count = 1 self.fields.append(field) field = pcl2.PointField() field.name = "intensity" field.offset = 12 field.datatype = pcl2.PointField.FLOAT32 field.count = 1 self.fields.append(field)
def handle_image(req): start = rospy.get_time() # Set up point cloud fields = [pc2.PointField() for _ in range(3)] fields[0].name = "x" fields[0].offset = 0 fields[0].datatype = pc2.PointField.FLOAT32 fields[0].count = 1 fields[1].name = "y" fields[1].offset = 4 fields[1].datatype = pc2.PointField.FLOAT32 fields[1].count = 1 fields[2].name = "z" fields[2].offset = 8 fields[2].datatype = pc2.PointField.FLOAT32 fields[2].count = 1 field_id = 3 fields.append(pc2.PointField()) fields[field_id].name = "intensity" fields[field_id].datatype = pc2.PointField.FLOAT32 fields[field_id].offset = 12 fields[field_id].count = 1 idx_intensity = field_id # add image img = bridge.imgmsg_to_cv2(req, desired_encoding="passthrough") # img = cv2.cvtColor(img, cv.CV_BGR2GRAY) img = cv2.resize(img, (w, h)) points = np.column_stack((points_xyz, np.flipud(np.ravel(img)))) points = points[np.logical_not(points[:, 3] == 0)] header = req.header header.frame_id = "base_footprint" if clear_space: xs = [p for p in points] n = 720 t = pi / n for i in range(200, n - 200): xs.append([sin(i * t) * 10, cos(i * t) * 10, 0, 255]) points = xs global cloud_out cloud_out = pc2.create_cloud(header, fields, points)
def ros_colorline(xyz): fields = [ pc2.PointField("x", 0, pc2.PointField.FLOAT32, 1), pc2.PointField("y", 4, pc2.PointField.FLOAT32, 1), pc2.PointField("z", 8, pc2.PointField.FLOAT32, 1), pc2.PointField("i", 12, pc2.PointField.FLOAT32, 1), ] xyzi = np.c_[xyz, np.array([[i] for i in range(len(xyz))])] header = rospy.Header() return pc2.create_cloud(header, fields, xyzi)
def PublishCloud(self, data): fields = [ pc2.PointField('x', 0, pc2.PointField.FLOAT32, 1), pc2.PointField('y', 4, pc2.PointField.FLOAT32, 1), pc2.PointField('z', 8, pc2.PointField.FLOAT32, 1), pc2.PointField('intensity', 12, pc2.PointField.FLOAT32, 1) ] a = PointCloud2 header = Header() header.stamp = rospy.Time.now() header.frame_id = "velodyne" msP = pc2.create_cloud(header, fields, data) self.publisher.publish(msP)
def __fields_from_spec(self): """ Function creates the message template from can spec """ self.fields = [] self.field_names = [] self.byte_offset = 0 # get fields from header message # first check whether modeBit is set if self.target_serv.messageSpec["header"]["modeBit"] is not None: # modeBit in header is set, retrieve length for i in range( 0, self.target_serv.messageSpec["header"]["modeBit"] ["end_bit"] + 1): elements = self.target_serv.messageSpec["header"][str(i)] for element in elements: self.fields.append( pc2.PointField(element, self.byte_offset, pc2.PointField.FLOAT32, 1)) self.field_names.append(element) self.byte_offset += 4 else: # no modeBit is set, set field to default ("0") elements = self.target_serv.messageSpec["header"]["0"] for element in elements: self.fields.append( pc2.PointField(element, self.byte_offset, pc2.PointField.FLOAT32, 1)) self.field_names.append(element) self.byte_offset += 4 # retrieve length of body message from spec for i in range(0, self.target_serv.messageSpec["body"]["maximum"] + 1): elements = self.target_serv.messageSpec["body"][str(i)] for element in elements: self.fields.append( pc2.PointField(element, self.byte_offset, pc2.PointField.FLOAT32, 1)) self.field_names.append(element) self.byte_offset += 4
def n2r(numpy_arr, msg): if msg == "Image": if numpy_arr.ndim == 2 or numpy_arr.shape[2] == 1: return bridge.cv2_to_imgmsg(numpy_arr, encoding="8U") else: return bridge.cv2_to_imgmsg(numpy_arr, encoding="rgb8") elif msg == "ImageBGR": return bridge.cv2_to_imgmsg(numpy_arr, encoding="bgr8") elif msg == "PointCloudXYZ": header = rospy.Header() return pc2.create_cloud_xyz32(header, np.array(numpy_arr)) elif msg == "PointCloudXYZI": header = rospy.Header() fields = [ pc2.PointField("x", 0, pc2.PointField.FLOAT32, 1), pc2.PointField("y", 4, pc2.PointField.FLOAT32, 1), pc2.PointField("z", 8, pc2.PointField.FLOAT32, 1), pc2.PointField("i", 12, pc2.PointField.FLOAT32, 1), ] return pc2.create_cloud(header, fields, np.array(numpy_arr)) else: raise NotImplementedError( "Not implemented from numpy array to {}".format(msg))
def __init__(self): self.fields = [ pc2.PointField('x', 0, pc2.PointField.FLOAT32, 1), pc2.PointField('y', 4, pc2.PointField.FLOAT32, 1), pc2.PointField('z', 8, pc2.PointField.FLOAT32, 1), pc2.PointField('Object_Number', 12, pc2.PointField.FLOAT32, 1), pc2.PointField('Speed', 16, pc2.PointField.FLOAT32, 1), pc2.PointField('Heading', 20, pc2.PointField.FLOAT32, 1), pc2.PointField('Quality', 24, pc2.PointField.FLOAT32, 1), pc2.PointField('Acceleration', 28, pc2.PointField.FLOAT32, 1) ] # define publisher for the pointcloud with the cartesian coordinates self.pub = rospy.Publisher('object_list', pc2.PointCloud2, queue_size=1) # define subscriber to pointcloud rospy.Subscriber("radar_data", pc2.PointCloud2, self.pc_callback)
def ros_colorline_trajectory(traj): fields = [ pc2.PointField("x", 0, pc2.PointField.FLOAT32, 1), pc2.PointField("y", 4, pc2.PointField.FLOAT32, 1), pc2.PointField("z", 8, pc2.PointField.FLOAT32, 1), pc2.PointField("roll", 12, pc2.PointField.FLOAT32, 1), pc2.PointField("pitch", 16, pc2.PointField.FLOAT32, 1), pc2.PointField("yaw", 20, pc2.PointField.FLOAT32, 1), pc2.PointField("i", 24, pc2.PointField.FLOAT32, 1), ] traji = np.c_[traj, np.mgrid[0 : len(traj)]] header = rospy.Header() return pc2.create_cloud(header, fields, traji)
def __projectLaser(self, scan_in, range_cutoff, channel_options): N = len(scan_in.ranges) ranges = np.array(scan_in.ranges) if (self.__cos_sin_map.shape[1] != N or self.__angle_min != scan_in.angle_min or self.__angle_max != scan_in.angle_max): rospy.logdebug("No precomputed map given. Computing one.") self.__angle_min = scan_in.angle_min self.__angle_max = scan_in.angle_max angles = scan_in.angle_min + np.arange(N) * scan_in.angle_increment self.__cos_sin_map = np.array([np.cos(angles), np.sin(angles)]) output = ranges * self.__cos_sin_map # Set the output cloud accordingly cloud_out = PointCloud2() fields = [pc2.PointField() for _ in range(3)] fields[0].name = "x" fields[0].offset = 0 fields[0].datatype = pc2.PointField.FLOAT32 fields[0].count = 1 fields[1].name = "y" fields[1].offset = 4 fields[1].datatype = pc2.PointField.FLOAT32 fields[1].count = 1 fields[2].name = "z" fields[2].offset = 8 fields[2].datatype = pc2.PointField.FLOAT32 fields[2].count = 1 idx_intensity = idx_index = idx_distance = idx_timestamp = -1 idx_vpx = idx_vpy = idx_vpz = -1 offset = 12 if (channel_options & self.ChannelOption.INTENSITY and len(scan_in.intensities) > 0): field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "intensity" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset + 4 fields[field_size].count = 1 offset += 4 idx_intensity = field_size # This is a hack!!! field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "ring" fields[field_size].datatype = pc2.PointField.UINT16 fields[field_size].offset = 20 fields[field_size].count = 1 if channel_options & self.ChannelOption.INDEX: field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "index" fields[field_size].datatype = pc2.PointField.INT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_index = field_size if channel_options & self.ChannelOption.DISTANCE: field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "distances" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_distance = field_size if channel_options & self.ChannelOption.TIMESTAMP: field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "stamps" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_timestamp = field_size if channel_options & self.ChannelOption.VIEWPOINT: field_size = len(fields) fields.extend([pc2.PointField() for _ in range(3)]) fields[field_size].name = "vp_x" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_vpx = field_size field_size += 1 fields[field_size].name = "vp_y" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_vpy = field_size field_size += 1 fields[field_size].name = "vp_z" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_vpz = field_size if range_cutoff < 0: range_cutoff = scan_in.range_max else: range_cutoff = min(range_cutoff, scan_in.range_max) points = [] for i in range(N): ri = scan_in.ranges[i] if ri < range_cutoff and ri >= scan_in.range_min: point = output[:, i].tolist() point.append(0) if idx_intensity != -1: point.append(scan_in.intensities[i]) if idx_index != -1: point.append(i) if idx_distance != -1: point.append(scan_in.ranges[i]) if idx_timestamp != -1: point.append(i * scan_in.time_increment) if idx_vpx != -1 and idx_vpy != -1 and idx_vpz != -1: point.extend([0 for _ in range(3)]) point.append(i) # hack for "ring" points.append(point) cloud_out = pc2.create_cloud(scan_in.header, fields, points) return cloud_out
def _make_point_field(num_field): msg_pf1 = pc2.PointField() msg_pf1.name = np.str('x') msg_pf1.offset = np.uint32(0) msg_pf1.datatype = np.uint8(7) msg_pf1.count = np.uint32(1) msg_pf2 = pc2.PointField() msg_pf2.name = np.str('y') msg_pf2.offset = np.uint32(4) msg_pf2.datatype = np.uint8(7) msg_pf2.count = np.uint32(1) msg_pf3 = pc2.PointField() msg_pf3.name = np.str('z') msg_pf3.offset = np.uint32(8) msg_pf3.datatype = np.uint8(7) msg_pf3.count = np.uint32(1) if num_field == 4: msg_pf4 = pc2.PointField() msg_pf4.name = np.str('node') msg_pf4.offset = np.uint32(16) msg_pf4.datatype = np.uint8(7) msg_pf4.count = np.uint32(1) return [msg_pf1, msg_pf2, msg_pf3, msg_pf4] elif num_field == 6: msg_pf4 = pc2.PointField() msg_pf4.name = np.str('intensity') msg_pf4.offset = np.uint32(16) msg_pf4.datatype = np.uint8(7) #float64 msg_pf4.count = np.uint32(1) msg_pf5 = pc2.PointField() msg_pf5.name = np.str('sem_label') msg_pf5.offset = np.uint32(20) msg_pf5.datatype = np.uint8(7) # 4 int16 msg_pf5.count = np.uint32(1) msg_pf6 = pc2.PointField() msg_pf6.name = np.str('inst_label') msg_pf6.offset = np.uint32(24) msg_pf6.datatype = np.uint8(7) # 4 int16 msg_pf6.count = np.uint32(1) return [msg_pf1, msg_pf2, msg_pf3, msg_pf4, msg_pf5, msg_pf6] # if num_field == 4: # fields = [PointField('x', 0, PointField.FLOAT32, 1), # PointField('y', 4, PointField.FLOAT32, 1), # PointField('z', 8, PointField.FLOAT32, 1), # PointField('node', 16, PointField.UINT32, 1), # ] # return fields # elif num_field == 6: # fields = [PointField('x', 0, PointField.FLOAT32, 1), # PointField('y', 4, PointField.FLOAT32, 1), # PointField('z', 8, PointField.FLOAT32, 1), # PointField('intensity', 12, PointField.FLOAT32, 1), # PointField('sem_label', 16, PointField.UINT32, 1), # PointField('inst_label', 20, PointField.UINT32, 1), # ] # return fields else: raise ValueError("wrong num_field.")
def __projectLaser(self, scan_in, range_cutoff, channel_options): N = len(scan_in.ranges) ranges = np.array(scan_in.ranges) if (self.__cos_sin_map.shape[1] != N or self.__angle_min != scan_in.angle_min or self.__angle_max != scan_in.angle_max): rospy.logdebug("No precomputed map given. Computing one.") self.__angle_min = scan_in.angle_min self.__angle_max = scan_in.angle_max angles = scan_in.angle_min + np.arange(N) * scan_in.angle_increment self.__cos_sin_map = np.array([np.cos(angles), np.sin(angles)]) output = ranges * self.__cos_sin_map # Set the output cloud accordingly cloud_out = PointCloud2() fields = [pc2.PointField() for _ in range(3)] fields[0].name = "x" fields[0].offset = 0 fields[0].datatype = pc2.PointField.FLOAT32 fields[0].count = 1 fields[1].name = "y" fields[1].offset = 4 fields[1].datatype = pc2.PointField.FLOAT32 fields[1].count = 1 fields[2].name = "z" fields[2].offset = 8 fields[2].datatype = pc2.PointField.FLOAT32 fields[2].count = 1 idx_intensity = idx_index = idx_distance = idx_timestamp = -1 idx_vpx = idx_vpy = idx_vpz = -1 offset = 12 if (channel_options & self.ChannelOption.INTENSITY and len(scan_in.intensities) > 0): field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "intensity" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_intensity = field_size if channel_options & self.ChannelOption.INDEX: field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "index" fields[field_size].datatype = pc2.PointField.INT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_index = field_size if channel_options & self.ChannelOption.DISTANCE: field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "distances" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_distance = field_size if channel_options & self.ChannelOption.TIMESTAMP: field_size = len(fields) fields.append(pc2.PointField()) fields[field_size].name = "stamps" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_timestamp = field_size if channel_options & self.ChannelOption.VIEWPOINT: field_size = len(fields) fields.extend([pc2.PointField() for _ in range(3)]) fields[field_size].name = "vp_x" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_vpx = field_size field_size += 1 fields[field_size].name = "vp_y" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_vpy = field_size field_size += 1 fields[field_size].name = "vp_z" fields[field_size].datatype = pc2.PointField.FLOAT32 fields[field_size].offset = offset fields[field_size].count = 1 offset += 4 idx_vpz = field_size if range_cutoff < 0: range_cutoff = scan_in.range_max else: range_cutoff = min(range_cutoff, scan_in.range_max) points = [] big_str = "\n" for i in range(N): ri = scan_in.ranges[i] # if ri < range_cutoff and ri >= scan_in.range_min: if True: point = output[:, i].tolist() point.append(0) p = point angle_increment = scan_in.angle_increment min_angle = scan_in.angle_min dist = ri idx = i if idx_intensity != -1: point.append(scan_in.intensities[i]) if idx_index != -1: point.append(i) if idx_distance != -1: point.append(scan_in.ranges[i]) if idx_timestamp != -1: point.append(i * scan_in.time_increment) if idx_vpx != -1 and idx_vpy != -1 and idx_vpz != -1: point.extend([0 for _ in range(3)]) points.append(point) big_str += " " + str(idx).zfill(2) + ": x: " + str( round(p[0], 2)) + ", y: " + str(round( p[1], 2)) + ", z: " + str(round(p[2], 2)) + " = " + str( round(dist, 2)) + "m (at " + str( round( degrees(idx * angle_increment + min_angle), 2)) + "deg)\n" rospy.loginfo("Projected cloud:") rospy.loginfo(big_str) cloud_out = pc2.create_cloud(scan_in.header, fields, points) return cloud_out