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)