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
