def __projectLaser(self, scan_in, range_cutoff, channel_options):
N = len(scan_in.ranges)
ranges = np.array(scan_in.ranges)
ranges = np.array([ranges, 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
cos_map = [np.cos(scan_in.angle_min + i * scan_in.angle_increment)
for i in range(N)]
sin_map = [np.sin(scan_in.angle_min + i * scan_in.angle_increment)
for i in range(N)]
self.__cos_sin_map = np.array([cos_map, sin_map])
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 = []
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)])
points.append(point)
cloud_out = pc2.create_cloud(scan_in.header, fields, points)
return cloud_out
def _publish_cloud(self, trajectory, cost, costs, mask_rotation):
header = Header(frame_id="map")
points = np.vstack(
(trajectory[:, -1, :2].T, np.zeros(cost.shape), costs, cost)).T
mask = mask_rotation
x, y = trajectory[0, 0, :2]
theta_rot = trajectory[mask, -1, 2:3].T
r = 0.1
points_rot = np.vstack(
(x + r * np.cos(theta_rot), y + r * np.sin(theta_rot),
np.zeros(cost[mask].shape), [c[mask]
for c in costs], cost[mask])).T
points_rot_filtered = points_rot[
points_rot[:, 3 + len(costs)] < DWAPlanner.LETHAL_COST]
points_filtered = points[points[:, 3 +
len(costs)] < DWAPlanner.LETHAL_COST]
points_filtered_out = points[
points[:, 3 + len(costs)] > DWAPlanner.LETHAL_COST - 1]
cost_msg = point_cloud2.create_cloud(header, self._fields,
points_filtered)
lethal_cost_msg = point_cloud2.create_cloud(header, self._fields,
points_filtered_out)
rotation_cost_msg = point_cloud2.create_cloud(header, self._fields,
points_rot_filtered)
try:
self._cost_pub.publish(cost_msg)
self._lethal_cost_pub.publish(lethal_cost_msg)
self._rotation_cost_pub.publish(rotation_cost_msg)
except rospy.ROSException as e:
rospy.logdebug("DWAPlanner: {}".format(e))
def serve(self, idx):
self.header.seq = idx
self.header.stamp = rospy.Time.from_sec(
self.dataset.timestamps[idx].total_seconds())
current_cloud = self.dataset.get_velo(idx)
if idx == 0:
# guess 0 pose at first time frame
tr, quat = np.zeros((3, )), np.array([0., 0., 0., 1.])
else:
# estimate coarse pose relative to the previous frame with model
prev_cloud = self.dataset.get_velo(idx - 1)
tr, quat = self.estimate_pose(prev_cloud, current_cloud)
gt_pose = self.dataset.poses[idx]
est_mat = trq2mat(tr, quat)
delta_gt_pose = delta_poses(gt_pose.copy(),
self.absolute_gt_pose.copy())
self.absolute_gt_pose = gt_pose
trans_error, rot_error = pose_error(delta_gt_pose, est_mat.copy())
self.tr_error_meter.update(trans_error)
self.rot_error_meter.update(rot_error)
# correct the axis system of the estimated pose
c_est_mat = kitti2rvizaxis(est_mat.copy())
c_tr, c_quat = mat2trq(c_est_mat)
cap_msg = CloudAndPose()
cap_msg.seq = idx
cap_msg.point_cloud2 = point_cloud2.create_cloud(
self.header, self.fields, [point for point in current_cloud])
cap_msg.init_guess = self.tq2tf_msg(*mat2trq(delta_gt_pose),
self.header, "est")
self.absolute_est_pose = add_poses(self.absolute_est_pose, c_est_mat)
est_mat_temp = self.absolute_est_pose.copy()
est_tf = self.mat2tf_msg(est_mat_temp, self.header, "est")
gt_tf = self.mat2tf_msg(kitti2rvizaxis(gt_pose.copy()), self.header,
"gt")
self.est_tf_pub.publish(est_tf)
self.gt_tf_pub.publish(gt_tf)
self.transform_broadcaster.sendTransform(gt_tf)
self.transform_broadcaster.sendTransform(est_tf)
self.cloud_pub.publish(
point_cloud2.create_cloud(Header(frame_id="gt_car"), self.fields,
[point for point in current_cloud]))
self.cap_pub.publish(cap_msg)
print(
"[{}] inference spent: {:.2f} ms\t\t| Trans : {}\t\t| GT Trans: {}\t\t| Trans error: {:.4f}\t\t| "
"Rot error: {:.4f}".format(
idx, self.infer_time_meter.avg, list(c_tr),
list(delta_gt_pose[:3, -1].reshape(3, )), trans_error,
rot_error))
self.rate.sleep()
def callback(self, req):
self.network.eval()
pc_msg = rospy.wait_for_message("/pc_preprocessing",PointCloud2)
points_list = []
color_list = []
for data in point_cloud2.read_points(pc_msg, skip_nans=True):
points_list.append([data[0], data[1], data[2]])
color_list.append(data[3])
point = np.asarray(points_list)
color_list = np.asarray(color_list)
if point.shape[0] < self.num_points:
row_idx = np.random.choice(point.shape[0], self.num_points, replace=True)
else:
row_idx = np.random.choice(point.shape[0], self.num_points, replace=False)
point_in = torch.from_numpy(point[row_idx]) ## need to revise
color_list = color_list[row_idx]
point_in = np.transpose(point_in, (1, 0))
point_in = point_in[np.newaxis,:]
inputs = Variable(point_in).type(torch.FloatTensor).cuda()
output = self.network(inputs)[0][0]
_point_list = []
_origin_list = []
for i in range(self.num_points):
s = struct.pack('>f' ,color_list[i])
k = struct.unpack('>l',s)[0]
pack = ctypes.c_uint32(k).value
r = (pack & 0x00FF0000)>> 16
g = (pack & 0x0000FF00)>> 8
b = (pack & 0x000000FF)
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, 255))[0]
#if output[i].argmax() == labels[i] and output[i].argmax() == 1:
if output[i].argmax() == 1:
#if output[i][0] < -0.1:
_point_list.append([inputs[0][0][i], inputs[0][1][i], inputs[0][2][i],rgb])
_origin_list.append([inputs[0][0][i], inputs[0][1][i], inputs[0][2][i],rgb])
header = Header()
header.stamp = rospy.Time.now()
header.frame_id ="camera_link"
fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('rgb', 12, PointField.UINT32, 1)]
pointcloud_pre = point_cloud2.create_cloud(header, fields, _point_list)
pointcloud_origin = point_cloud2.create_cloud(header, fields, _origin_list)
self.prediction.publish(pointcloud_pre)
self.origin.publish(pointcloud_origin)
return "Finish" + ", point num: " + str(len(_point_list)) + ", origin num: " + str(point.shape[0])
def save_velo_data(bag, kitti, velo_frame_id, velo_topic):
print("Exporting Velodyne data")
velo_data_dir = os.path.join(kitti.data_path, 'velodyne')
velo_filenames = sorted(os.listdir(velo_data_dir))
datatimes = kitti.timestamps
iterable = zip(datatimes, velo_filenames)
bar = progressbar.ProgressBar()
for dt, veloname in bar(iterable):
if dt is None:
continue
velo_filename = os.path.join(velo_data_dir, veloname)
veloscan = (np.fromfile(velo_filename,
dtype=np.float32)).reshape(-1, 4)
header = Header()
header.frame_id = velo_frame_id
header.stamp = rospy.Time.from_sec(float(dt))
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)
]
pcl_msg = pcl2.create_cloud(header, fields, veloscan)
bag.write(velo_topic, pcl_msg, t=pcl_msg.header.stamp)
def callback(data):
global init_timer
global loop_timer
global ends_timer
# built-in function to read points from a cloud
generator = pc2.read_points(data, skip_nans=True)
"""
# generate a list of points if each axis is less than 1 meter from the camera
t = time.time()
# new_array = [point for point in generator if not abs(point[0]) > 1 and not abs(point[1]) > 1 and not abs(point[2]) > 1]
new_array = [point for point in generator if not sqrt(point[0]**2 + point[1]**2 + point[2]**2) > 1]
time_list.append(time.time() - t)
rospy.loginfo("min: " + str(min(time_list)) + " | max: " + str(max(time_list)) + " | avg: " + str(mean(time_list)))
"""
"""
# multiprocessing test
t = time.time()
new_array = [point for point in list(pool.imap_unordered(is_close, generator, chunksize=100000)) if point is not None]
rospy.loginfo(time.time() - t)
"""
t = time.time()
new_array = list(pool.imap_unordered(is_close, generator, chunksize=768))
time_list.append(time.time() - t)
rospy.loginfo("min: " + str(min(time_list)) + " | max: " +
str(max(time_list)) + " | avg: " + str(mean(time_list)))
# generate a new cloud with the old header and fields but with the new array
new_cloud = pc2.create_cloud(data.header, data.fields, new_array)
pub.publish(new_cloud)
def callback(self, ros_cloud):
"""
will be called when received point cloud from realsense
params:
ros_cloud: point cloud of ros_msg pointcloud2 datatype
return:
publish the points in topic 'filtered_pointcloud'
"""
self.pointcloud = []
# apply hsv filter on point cloud
self.received_ros_cloud = ros_cloud
self.open3d_pointcloud = self.filter(self.received_ros_cloud)
for itr in range(len(self.open3d_pointcloud)):
self.pointcloud.append(self.open3d_pointcloud[itr][:3].tolist())
self.pc2 = point_cloud2.create_cloud(self.header, self.FIELDS_XYZ,
self.pointcloud)
my_pcl_pcd_converter.pc2.header.stamp = rospy.Time.now()
my_pcl_pcd_converter.pub.publish(my_pcl_pcd_converter.pc2)
def to_msg(open3d_cloud, frame_id=None, stamp=None):
header = Header()
if stamp is not None:
header.stamp = stamp
if frame_id is not None:
header.frame_id = frame_id
o3d_asarray = np.asarray(open3d_cloud.points)
o3d_x = o3d_asarray[:, 0]
o3d_y = o3d_asarray[:, 1]
o3d_z = o3d_asarray[:, 2]
cloud_data = np.core.records.fromarrays([o3d_x, o3d_y, o3d_z],
names='x,y,z')
if not open3d_cloud.colors: # XYZ only
fields = FIELDS_XYZ
else: # XYZ + RGB
fields = FIELDS_XYZRGB
color_array = np.array(np.floor(np.asarray(open3d_cloud.colors) * 255),
dtype=np.uint8)
o3d_r = color_array[:, 0]
o3d_g = color_array[:, 1]
o3d_b = color_array[:, 2]
cloud_data = np.lib.recfunctions.append_fields(cloud_data,
['r', 'g', 'b'],
[o3d_r, o3d_g, o3d_b])
cloud_data = ros_numpy.point_cloud2.merge_rgb_fields(cloud_data)
return pc2.create_cloud(header, fields, cloud_data)
def pointcloud_ge(self, road_points):
'''
msg = masked rgb image
depth = depth image
'''
# convert massage to numpy array
pointcloud = road_points
print("type_pointcloudxyz", type(pointcloud))
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
]
header = Header()
#header.frame_id = 'sensor/lidar/velodyne/fl'
header.frame_id = 'front_color_rect'
point_generate = pc2.create_cloud(header, fields, pointcloud)
point_generate.header.stamp = rospy.Time.now()
print("3333333333333")
return point_generate
def transform_points_robot(points_msg):
#points_transformed = []
for p in point_cloud2.read_points(points_msg, skip_nans=True):
x_translated = round(p[0], 3) + robot0_pose[0]
y_translated = round(p[1], 3) + robot0_pose[1]
z_translated = round(p[2], 3)
rotation_angle = math.radians(robot0_pose[2])
x_transformed = x_translated * math.cos(
rotation_angle) + y_translated * math.sin(rotation_angle)
y_transformed = y_translated * math.cos(
rotation_angle) - x_translated * math.sin(rotation_angle)
pt = [x_transformed, y_transformed, z_translated]
points_transformed.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)
]
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = "world"
pc2 = point_cloud2.create_cloud(header, fields, points_transformed)
pub.publish(pc2)
def publish(self):
"""publish a single point cloud """
if rospy is None:
print('distance_m_1', self.distance_m_1.flatten())
print('intensity_1', self.intensity_1.flatten())
print('labels_1', self.labels_1.flatten())
return
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'base'
# http://wiki.ros.org/rviz/DisplayTypes/PointCloud
r, g, b = self.get_rgb(self.labels_1.flatten())
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('distance', 12, PointField.FLOAT32, 1),
PointField('intensity', 16, PointField.FLOAT32, 1),
PointField('r', 20, PointField.FLOAT32, 1),
PointField('g', 24, PointField.FLOAT32, 1),
PointField('b', 28, PointField.FLOAT32, 1)
]
points = list(
zip(self.sensorX_1.flatten(), self.sensorY_1.flatten(),
self.sensorZ_1.flatten(), self.distance_m_1.flatten(),
self.intensity_1.flatten(), r, g, b))
cloud = pc2.create_cloud(header, fields, points)
self.ros_publisher.publish(cloud)
def publishdata(x, y, z):
pub = rospy.Publisher("mypoints", PointCloud2, queue_size=1)
pts = []
for i in range(len(x)):
pt = [float(x[i]), float(y[i]), float(z[i])]
pts.append(pt)
#print(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
]
header = Header()
header.frame_id = "map"
mypc2 = pc2.create_cloud(header, fields, pts)
mypc2.header.stamp = rospy.Time.now()
pub.publish(mypc2)
def publishPointCloud(self, thisImage, pubHandle, timeStamp, height):
if pubHandle.get_num_connections() == 0:
return
# multi-channel range image, the first channel is range
if len(thisImage.shape) == 3:
thisImage = thisImage[:, :, 0]
lengthList = thisImage.reshape(image_rows_high * image_cols)
lengthList[lengthList > max_range] = 0.0
lengthList[lengthList < min_range] = 0.0
x = np.sin(self.horizonAngle) * np.cos(self.verticalAngle) * lengthList
y = np.cos(self.horizonAngle) * np.cos(self.verticalAngle) * lengthList
z = np.sin(self.verticalAngle) * lengthList + height
points = np.column_stack((x, y, z, self.intensity))
# delete points that has range value 0
points = np.delete(
points, np.where(lengthList == 0), axis=0
) # comment this line for visualize at the same speed (for video generation)
header = Header()
header.frame_id = 'base_link'
header.stamp = timeStamp
laserCloudOut = pc2.create_cloud(header, self.fields, points)
pubHandle.publish(laserCloudOut)
def callback(self, msg):
points = []
for cluster in msg.clusters:
index = 0
if cluster.count > self._first_cluster_threshold:
index = index + 1
if cluster.count > self._second_cluster_threshold:
index = index + 1
r = 0
g = 0
b = 0
a = 255
if index == 0:
r = 255
if index == 1:
g = 255
if index == 2:
b = 255
rgb = struct.unpack('I', struct.pack('BBBB',b,g,r,a))[0]
for point in cluster.points:
pt = [point.point_3d.x, point.point_3d.y, point.point_3d.z, rgb]
points.append(pt)
fields = [PointField('x',0,PointField.FLOAT32,1),
PointField('y',4,PointField.FLOAT32,1),
PointField('z',8,PointField.FLOAT32,1),
PointField('rgba',12,PointField.UINT32,1)]
header = Header()
header.frame_id = "map"
rt = pc2.create_cloud(header, fields, points)
self._pub.publish(rt)
def publish_points(self):
point_cloud = pc2.create_cloud(
HEADER, FIELDS, POINTS) #creation of sensor_msgs/PointCloud2
r = rospy.Rate(1)
while not rospy.is_shutdown():
self.publisher.publish(point_cloud)
r.sleep()
def callback(arg):
points = []
for k in range(len(arg.points)):
x = float(arg.points[k].x)
y = float(arg.points[k].y)
z = float(0)
r = int(255.0)
g = int(255.0)
b = int(255.0)
a = 255
rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [x, y, z, rgb]
points.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 0, PointField.FLOAT32, 1),
PointField('z', 0, PointField.FLOAT32, 1),
# PointField('rgb', 12, PointField.UINT32, 1),
PointField('rgba', 12, PointField.UINT32, 1),
]
header = Header()
header.frame_id = "sonar"
pc2 = point_cloud2.create_cloud(header, fields, points)
pc2.header.stamp = rospy.Time.now()
pub.publish(pc2)
def convertToPointCloud2(self, cloud, normals=None):
'''
Transform the Infinitam volume to a point cloud in the /base frame.
@type cloud:
@param cloud:
'''
header = HeaderMsg()
header.frame_id = "/base"
header.stamp = rospy.Time.now()
if normals is None:
return point_cloud2.create_cloud_xyz32(header, cloud)
# concatenate xyz and normals vertically
pts = numpy.zeros((cloud.shape[0],6))
pts[:,0:3] = cloud[:,0:3]
pts[:,3:6] = normals[:,0:3]
# create message
fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('normal_x', 12, PointField.FLOAT32, 1),
PointField('normal_y', 16, PointField.FLOAT32, 1),
PointField('normal_z', 20, PointField.FLOAT32, 1)]
return point_cloud2.create_cloud(header, fields, pts)
def pc_callback(self, data):
cloud_points = list(
pc2.read_points(data, skip_nans=True, field_names=None))
# Get the index of the needed fields
for field, i in zip(data.fields, range(len(data.fields))):
if field.name == "Object_Number":
obj_idx = i
if field.name == "x_Point":
x_idx = i
if field.name == "y_Point":
y_idx = i
if field.name == "Speed":
speed_idx = i
if field.name == "Heading":
head_idx = i
if field.name == "Quality":
qual_idx = i
if field.name == "Acceleration":
acc_idx = i
points = []
for single_point in cloud_points:
x = single_point[x_idx]
y = single_point[y_idx]
obj_num = single_point[obj_idx]
speed = single_point[speed_idx]
heading = single_point[head_idx]
quality = single_point[qual_idx]
acc = single_point[acc_idx]
points.append([x, y, 0, obj_num, speed, heading, quality, acc])
cloud_msg = pc2.create_cloud(data.header, self.fields, points)
self.pub.publish(cloud_msg)
def save_velo_data(bag, kitti, velo_frame_id, topic):
print("Exporting velodyne data")
velo_path = os.path.join(kitti.data_path, 'velodyne_points')
velo_datetimes = read_timestamps(velo_path)
for dt, velo_filename in tqdm(list(zip(velo_datetimes, kitti.velo_files))):
if dt is None:
continue
# read binary data
scan = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4)
# create header
header = Header()
header.frame_id = velo_frame_id
header.stamp = to_rostime(dt)
# fill PCL msg
# uses the PCL PointXYZI struct's field names
# http://docs.pointclouds.org/1.7.0/point__types_8hpp_source.html#l00380
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)
]
pcl_msg = pcl2.create_cloud(header, fields, scan)
bag.write(topic + '/pointcloud', pcl_msg, t=pcl_msg.header.stamp)
def pointcloud2withrgb(self, points, colors, stamp, frame_id, seq):
points_array = []
for i in range(points.shape[0]):
x, y, z = points[i]
color = colors[i] * 255
color = color.astype(int)
hex_r = (0xff & color[0]) << 16
hex_g = (0xff & color[1]) << 8
hex_b = (0xff & color[2])
hex_rgb = hex_r | hex_g | hex_b
float_rgb = struct.unpack('f', struct.pack('i', hex_rgb))[0]
# r, g, b = color.astype(int)
# a = 0
# rgb = struct.unpack('I', struct.pack('BBBB', b, g, r, a))[0]
pt = [x, y, z, float_rgb]
points_array.append(pt)
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgb', 12, PointField.FLOAT32, 1),
]
header = Header()
header.frame_id = frame_id
msg = pc2.create_cloud(header, fields, points_array)
msg.header.stamp = stamp
return msg
def filterPointCloud2(msg, radius=None, swap='xyz'):
swap = swap.lower()
order = {'x': 0, 'y': 1, 'z': 2}
for i in range(len(swap)):
ival = swap[i]
order[ival] = i
outData = []
for p in pc2.read_points(msg, skip_nans=True):
x = p[0]
y = p[1]
z = p[2]
p = list(p)
p[order['x']] = x
p[order['y']] = y
p[order['z']] = z
p = tuple(p)
dst = math.sqrt(x**2 + y**2 + z**2)
if (radius is not None and dst > radius):
continue
outData.append(p)
msg.header.frame_id = "laser_link"
cld = pc2.create_cloud(msg.header, msg.fields, outData)
return cld
def save_velo_data(bag, kitti, velo_frame_id, topic):
print("Exporting velodyne data")
velo_path = os.path.join(kitti.data_path, 'velodyne_points')
velo_data_dir = os.path.join(velo_path, 'data')
velo_filenames = sorted(os.listdir(velo_data_dir))
with open(os.path.join(velo_path, 'timestamps.txt')) as f:
lines = f.readlines()
velo_datetimes = []
for line in lines:
if len(line) == 1:
continue
dt = datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f')
velo_datetimes.append(dt)
iterable = zip(velo_datetimes, velo_filenames)
bar = progressbar.ProgressBar()
count = 0
for dt, filename in bar(iterable):
if dt is None:
continue
velo_filename = os.path.join(velo_data_dir, filename)
# read binary data
scan = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4)
# get ring channel
depth = np.linalg.norm(scan, 2, axis=1)
pitch = np.arcsin(scan[:, 2] / depth) # arcsin(z, depth)
fov_down = -24.8 / 180.0 * np.pi
fov = (abs(-24.8) + abs(2.0)) / 180.0 * np.pi
proj_y = (pitch + abs(fov_down)) / fov # in [0.0, 1.0]
proj_y *= 64 # in [0.0, H]
proj_y = np.floor(proj_y)
proj_y = np.minimum(64 - 1, proj_y)
proj_y = np.maximum(0, proj_y).astype(np.int32) # in [0,H-1]
proj_y = proj_y.reshape(-1, 1)
scan = np.concatenate((scan, proj_y), axis=1)
# create header
header = Header()
header.frame_id = velo_frame_id
header.stamp = rospy.Time.from_sec(
float(datetime.strftime(dt, "%s.%f")))
# fill pcl msg
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('ring', 16, PointField.UINT16, 1)
]
pcl_msg = pcl2.create_cloud(header, fields, scan)
pcl_msg.is_dense = True
# print(pcl_msg)
bag.write(topic, pcl_msg, t=pcl_msg.header.stamp)
def add_trunk_data(self,pcl_LaserScan,dif_start,dif_end):
gen_data = pc2.read_points(pcl_LaserScan, field_names=None, skip_nans=True)
trunk_points = []
if self.trunk_pcl:
gen_trunk = pc2.read_points(self.trunk_pcl, field_names=None,skip_nans=True)
for p in gen_trunk:
trunk_points.append(list(p))
scan_points = []
for idx, p in enumerate(gen_data):
if idx >= dif_start and idx <= dif_end:
point_i_3d = list(p)
point_i_3d[2] = self.height_sign * point_i_3d[self.height_axis] + self.height_offset
point_i_3d[self.height_axis] = self.scanN * self.increment
scan_points.append(point_i_3d)
trunk_points.append(point_i_3d)
if self.save_raw_scan:
self.raw_scan.append(scan_points)
self.trunk_pcl = pc2.create_cloud(pcl_LaserScan.header, pcl_LaserScan.fields, trunk_points)
# show mean of points
scan_block_mean = np.mean(np.array(scan_points),axis = 0)
xy_mean = np.array([0,0])
try:
xy_mean[self.height_axis] = ( scan_block_mean[2] - self.height_offset ) * self.height_sign
except:
import pdb; pdb.set_trace() # XXX BREAKPOINT
pass
xy_mean[1-self.height_axis] = scan_block_mean[1-self.height_axis]
def talker():
pcl_pub = rospy.Publisher("/nimbus_ros/pointcloud", PointCloud2)
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),
]
rospy.init_node('nimbus_ros', anonymous=True)
rospy.loginfo("Initializing nimbus-ros publisher node...")
#connect to nimbus-sever
cli = NimbusClient.NimbusClient(
rospy.get_param('/nimbus_ip', '192.168.0.69'))
time.sleep(0.5)
#main loop
while not rospy.is_shutdown():
#get image data from nimbus
header, (ampl, radial, x, y, z, conf) = cli.getImage(invalidAsNan=True)
#convert pointcloud data for point_cloud2
cloud = np.transpose(
np.vstack((x.flatten(), y.flatten(), z.flatten(),
ampl.flatten()))).tolist()
header = Header()
header.frame_id = "nimbus"
pc2 = point_cloud2.create_cloud(header, fields, cloud)
pc2.header.stamp = rospy.Time.now()
pcl_pub.publish(pc2)
def __init__(self):
#ros node
rospy.init_node('liverGrid', anonymous=True)
# publisher
self.pub1 = rospy.Publisher("liverGrid", PointCloud2, queue_size=2)
self.pub2 = rospy.Publisher("liverGridNorm", PoseArray, queue_size=2)
self.rate = rospy.Rate(10)
# point cloud2
self.point_cloud = []
self.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgba', 12, PointField.UINT32, 1)
]
self.header = Header()
self.header.frame_id = "PSM1_psm_base_link" # the 3dpcl is in a new frame
self.pc2 = point_cloud2.create_cloud(self.header, self.fields,
self.point_cloud)
self.point_nparray = np.array([])
self.normal_vectors = PoseArray()
self.normal_vectors.header = self.header
def calculate_sand_volume():
sand_n = 1
path = '4-28-lab_pcl_result'
volume0, optimized_points_0 = volume_from_bag(path + '/empty.bag',
path + '/empty_optimized')
volume1, optimized_points_1 = volume_from_bag(
path + '/sand' + str(sand_n) + '.bag',
path + '/sand' + str(sand_n) + '_optimized')
sand_v = volume1 - volume0
error = (sand_v - 3.93) / 3.93
print "sand volume = ", volume1 - volume0, " ", error
empty_sand_points = optimized_points_0
empty_sand_points.extend(optimized_points_1)
empty_model_bag = rosbag.Bag(path + '/empty.bag')
msg_gen = empty_model_bag.read_messages(topics='trunk_pcl')
for topic, msg, t in msg_gen:
empty_sand_pcl = pc2.create_cloud(msg.header, msg.fields,
empty_sand_points)
empty_sand_Bag = rosbag.Bag(
path + '/empty_sand' + str(sand_n) + '.bag', 'w')
empty_sand_Bag.write('truck_pcl', empty_sand_pcl)
empty_sand_Bag.close()
print
"write: " + path + "/empty_sand" + str(sand_n) + ".bag"
def publish(self):
if not use_rviz:
return
header = Header()
header.frame_id = self.frame_id
msg = pc2.create_cloud(header, self.fields, self.points)
self.publisher.publish(msg)
def create_cloud_xyzil32(self, header, points):
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('label', 16, PointField.FLOAT32, 1)]
return pc2.create_cloud(header, fields, points)
def PublishPC2callback(self, data):
#data is the pc
pc = point_cloud2.read_points_list(data, skip_nans=True)
#fetch xyz
xyz = np.array(pc)[:, 0:3]
#check if it is inside hull
isInHull = in_hull(xyz, self.roipoly)
#pick indexes
xyz = xyz[np.where(isInHull == True)]
#setup pointfield
fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
]
#print points
header = data.header
pc2 = point_cloud2.create_cloud(header, fields, xyz)
#pctemp = data
#while not rospy.is_shutdown():
pc2.header.stamp = rospy.Time.now()
self.pub.publish(pc2)
print("Published")
def publish_pc(np_array, frame_id):
header = Header()
header.stamp = rospy.Time()
header.frame_id = frame_id
x = np_array[:, 0].reshape(-1)
y = np_array[:, 1].reshape(-1)
z = np_array[:, 2].reshape(-1)
# if intensity field exists
if np_array.shape[1] == 4:
i = np_array[:, 3].reshape(-1)
else:
i = np.zeros((np_array.shape[0], 1)).reshape(-1)
cloud = np.stack((x, y, z, i))
# point cloud segments
# 4 PointFields as channel description
msg_segment = pc2.create_cloud(header=header,
fields=_make_point_field(4),
points=cloud.T)
# publish to /velodyne_points_modified
pub_velo.publish(msg_segment) # DEBUG
def create_cloud_xyzrgb(header, xyzrgb):
fields = [
PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
PointField(name='rgba', offset=16, datatype=PointField.UINT32, count=1),
]
return create_cloud(header, fields, xyzrgb)
def do_transform_cloud(cloud, transform):
t_kdl = transform_to_kdl(transform)
points_out = []
for p_in in read_points(cloud):
p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
points_out.append(p_out)
res = create_cloud(transform.header, cloud.fields, points_out)
return res
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 = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
img = cv.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 create_colored_cloud(pts, color):
ptsimage = pts.reshape((-1, 3))
rgb = color.reshape((-1, 3))
argb = np.c_[rgb, np.zeros(shape=(rgb.shape[0], 1), dtype='u1')]
pcpack = np.ndarray(buffer=argb, dtype='f4', shape=(argb.shape[0], 1))
pc = pc2.create_cloud(images['left_info'].header,
[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("rgb", 12, pc2.PointField.FLOAT32, 1)],
np.c_[ptsimage, pcpack])
return pc
def create_cloud_xyzrgb(header, points):
"""
Create a L{sensor_msgs.msg.PointCloud2} message with 3 float32 fields (x, y, z).
@param header: The point cloud header.
@type header: L{std_msgs.msg.Header}
@param points: The point cloud points.
@type points: iterable
@return: The point cloud.
@rtype: L{sensor_msgs.msg.PointCloud2}
"""
fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgb', 12, PointField.UINT32, 1)]
return pc2.create_cloud(header, fields, points)
def transform_PointCloud2(cloud, transform, new_frame):
""" transforms all the points, returns new pc2.
transform: Pose object
"""
if isinstance(transform, Pose):
transform = transform.as_homog_matrix()
print transform
def transform_point(pt):
return np.dot(transform, np.array([pt[0], pt[1], pt[2], 1]))[0:3]
pts = [tuple(transform_point(p[:3])) + p[3:] for p in pc2.read_points(cloud,
['x', 'y', 'z', 'rgb'])]
header = copy.deepcopy(cloud.header)
header.frame_id = new_frame
fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
PointField('rgb', 12, PointField.FLOAT32, 1)]
newcloud = pc2.create_cloud(header, fields, pts)
#newcloud = pc2.create_cloud_xyz32(header, pts)
return newcloud
f0 = '/map'
f1 = '/%s/base'%name
f2 = '/%s/velodyne'%name
path1 = 'cloud.%05i.txt'
path2 = 'velodyneShot.pos.%04i'
base = time.time()
with rosbag.Bag('%s.bag'%name, 'w') as bag:
for seq in range(begin, end+1):
with open(path1%seq) as f:
lines = f.readlines()
points = [map(float, line.split()) for line in lines]
stamp = rospy.Time.from_sec(base + 0.1 * seq) # FIXME
header = Header(seq=seq, stamp=stamp, frame_id=f2)
pc2 = point_cloud2.create_cloud(header, fields, points)
bag.write(f2, pc2, stamp)
with open(path2%seq) as f:
lines = f.readlines()
parser = lambda label: matrix(*map(float, [line for line in lines \
if line.startswith(label)][0].strip().split()[2:]))
sensor_to_main = parser('sensorToMain = ')
main_to_origin = parser('mainToOrigin = ')
bag.write('/tf', tfm(sensor_to_main, f1, f2, stamp), stamp)
bag.write('/tf', tfm(main_to_origin, f0, f1, stamp), stamp)
def get_pcd(self, depth_img, grasp_pts, grasp_pts_list=None):
"Get points cloud from depth and grasp_pts"
cc = None
# Entire pcd of image
points_entire_img = self.get_points(depth_img)
# Get polygon points
for _, elem in enumerate(grasp_pts_list):
cci, rri = polygon(elem[:, 0], elem[:, 1])
if cc is None:
cc = cci
rr = rri
else:
cc = np.append(cc, cci)
rr = np.append(rr, rri)
# print(cc.shape)
# cc, rr = polygon(grasp_pts[:,0], grasp_pts[:,1])
# Convert to points
num_points = len(rr)
points = np.zeros((num_points, 4), np.float32)
for i in range(0, num_points):
depth = depth_img[rr[i], cc[i]]
points[i] = self.get_point(depth, rr[i], cc[i])
points_entire_img[rr[i] * 640 + cc[i]] = points[i]
# To pcl2 format
header, fields = self.pcl_head_field()
#pcl2_pcd = pcl2.create_cloud_xyz32(header, points)
#grasp_pcd = pcl.PointCloud(np.array(points, dtype=np.float32))
#grasp_pcd = pcl.PointCloud_PointXYZRGBA(points)
#grasp_pcd = pcl.PointCloud_PointXYZRGB(points)
#grasp_pcd = 0
pcl2_pcd = pcl2.create_cloud(header, fields, points)
pcl2_pcd = pcl2.create_cloud(header, fields, points_entire_img)
grasp_pcd = 0
return grasp_pcd, pcl2_pcd
def _serialize_numpy(self, buff):
"""
wrapper for factory-generated class that passes numpy module into serialize
"""
# for Image msgs
if self._type == 'sensor_msgs/Image':
self.data = bridge.cv_to_imgmsg(cv.fromarray(self.data), encoding=self.encoding).data
# for PointCloud2 msgs
if self._type == 'sensor_msgs/PointCloud2':
print 'Cloud is being serialized...'
# Pack each RGB triple into a single float
_rgb2float_vectorized = numpy.vectorize(_rgb2float)
rgb = _rgb2float_vectorized(self.data.rgb[:, :, 0],
self.data.rgb[:, :, 1],
self.data.rgb[:, :, 2])
rgb[:, :320] = 0.0 # turn left half WHITE
# Reshape to a list of (x, y, z, rgb) tuples
xyz = self.data.xyz.reshape(self.height * self.width, 3)
rgb = rgb.reshape(self.height * self.width)
rgb = numpy.expand_dims(rgb, 1) # insert blank 2nd dimension (for concatenation)
self.data = numpy.concatenate((xyz, rgb), axis=1)
# Make the PointCloud2 msg
self.header.stamp = rospy.Time.now()
self = point_cloud2.create_cloud(self.header, self.fields, self.data)
return self.serialize_numpy(buff, numpy) # serialize (with numpy wherever possible)
def _cloud_cb(self, cloud):
points = np.array(list(read_points(cloud)))
if points.shape[0] == 0:
return
pos = points[:,0:3]
cor = np.reshape(points[:,-1], (points.shape[0], 1))
# Get 4x4 matrix which transforms point cloud co-ords to odometry frame
try:
points_to_map = self.tf.asMatrix('/lasths', cloud.header)
except tf.ExtrapolationException:
return
transformed_points = points_to_map.dot(np.vstack((pos.T, np.ones((1, pos.shape[0])))))
transformed_points = transformed_points[:3,:].T
self.seq += 1
header = Header()
header.seq = self.seq
header.stamp = rospy.Time.now()
header.frame_id = '/lasths'
self.cloud = np.vstack((self.cloud, np.hstack((transformed_points, cor))))
if self.seq % 30 == 0:
print "plup!"
self.cloud = np.zeros((0, 4))
output_cloud = create_cloud(header, cloud.fields, self.cloud)
self.cloud_pub.publish(output_cloud)
