def votenet_callback(req): points = pointcloud2_to_array(req.pc) result_points, pred_map_cls = point_cloud_to_detections(points) pc = array_to_pointcloud2( np.asarray(result_points, dtype=[('x', '<f4'), ('y', '<f4'), ('z', '<f4')])) msg = VotenetResponse(pc=pc) return msg
def read_pointcloud_frames(topic_name, bagfile): '''returns a list of tuples: (stamp,numpy_array) where stamp is the time and numpy_array is a labelled array with the pointcloud data''' frames = [] for topic, msg, t in bagfile.read_messages(topics=[topic_name]): pc_array = point_cloud2.pointcloud2_to_array(msg) frames.append((msg.header.stamp, pc_array)) return frames
def save_cloud(self, seq): cloud_img = get_xyz_points(pointcloud2_to_array(self.point_cloud), remove_nans=False, dtype=np.float32) # DO NOT COPY THE POINT CLOUD ANYMORE. ALWAYS USES RESHAPED VIEWS cloud_xyz = cloud_img.view() cloud_xyz.shape = (-1, 3) cloud = pcl.PointCloud(cloud_xyz) path = os.path.join(self.folder, '{:04d}_cloud.pcd'.format(seq)) cloud.to_file(path)
def are_clouds_equal(self, ros_cloud, open3d_cloud): open3d_xyz_asarray = np.asarray(open3d_cloud.points) ros_xyz_asarray = pointcloud2_to_xyz_array(ros_cloud) self.assertEqual(1, ros_cloud.height) self.assertEqual(ros_cloud.width, open3d_xyz_asarray.shape[0]) self.assertTrue(np.all(np.isclose(open3d_xyz_asarray, ros_xyz_asarray))) if open3d_cloud.has_colors(): open3d_colors_asarray = np.asarray(open3d_cloud.colors) * 255 ros_colors_asarray = get_rgb_points( split_rgb_field(pointcloud2_to_array(ros_cloud))) self.assertTrue( np.all(np.isclose(open3d_colors_asarray, ros_colors_asarray)))
def process(self, pc_msg, objects_msg): pc = pointcloud2_to_array(pc_msg) num_mask = np.isfinite(pc['x']) \ & np.isfinite(pc['y']) \ & np.isfinite(pc['z']) self.track_prev = {} for track_id in self.track_current: self.track_prev[track_id] = self.track_current[track_id] self.track_current = {} for object_msg in objects_msg.objects: if object_msg.track_id < 1: continue obj_mask = rle_msg_to_mask(object_msg.rle).astype(np.bool) obj_pc = s2u(pc[num_mask & obj_mask][['x', 'y', 'z']]) if len(obj_pc) < 50: continue local_center = clastering(obj_pc) self.track_current[object_msg.track_id] = { 'n_frame': self.n_frame, 'center': local_center, } all_markers = create_marker_array(self.track_current, self.colors, pc_msg.header, 1., 0) all_markers.extend( create_marker_array(self.track_prev, self.colors, pc_msg.header, 1.2, len(self.track_current))) self.centers_pub.publish(MarkerArray(all_markers)) self.br.publish(self.n_frame, pc_msg.header) self.n_frame += 1
def callbackROS(velo_ros): global index if index in toLook: arr = rospc2.pointcloud2_to_array(velo_ros) velo = get_xyzi_points(arr) #velo = passthrough(velo, 0, 70, -40, 40, -3.0, 3.5) intensities = velo[:, 3] xyz = np.hstack((velo[:, 0:3], np.ones((velo.shape[0], 1)))) #xyz = xyz.dot(T_cam_velo.T) #t = np.copy(xyz) #xyz[:,0] = t[:,2] #xyz[:,1] = t[:,0] #xyz[:,2] = t[:,1] xyz[:, 3] = intensities / 255 indexs = int(np.where(toLook == index)[0]) indexs = str(indexs).zfill(4) xyz.reshape((xyz.size)) #one dimension xyz.astype(np.float32).tofile(folder + 'pc' + indexs + '.bin') index += 1
def main(): listener = RsListener() rrate = rospy.Rate(100) try: while not rospy.is_shutdown(): if listener.pcmsg is not None: cv_image = listener.bridge.imgmsg_to_cv2( listener.imgmsg, desired_encoding='passthrough') gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) tags = listener.detector.detect(gray) if len(tags) > 0: # try: corners = np.transpose(tags[0].corners) # print("found apriltags:") # print("no: ", len(tags)) # print(tags) # print(corners) x = corners[0, :] y = corners[1, :] x = [int(temp) for temp in x] y = [int(temp) for temp in y] cx = int(tags[0].center[0]) cy = int(tags[0].center[1]) x.append(cx) y.append(cy) points = [] pcarray = pc2_np.pointcloud2_to_array(listener.pcmsg) points = [] for ix, iy in zip(x, y): p = [] ptem = pcarray[iy, ix] for idim in range(3): p.append(ptem[idim]) # p = pcarray[iy, ix][0:3] points.append(p) print(points) if not listener.points: listener.points = [points] listener.tfs = [listener.tf] else: listener.points.append(points) listener.tfs.append(listener.tf) listener.tf = None listener.flag_record = False # except IndexError: else: listener.flag_record = True pass listener.pcmsg = None listener.imgmsg = None else: rrate.sleep() except rospy.ROSInterruptException: return except KeyboardInterrupt: return
if i == 0: continue bag_in = rosbag.Bag(sys.argv[i], 'r') name = sys.argv[i][:sys.argv[i].rindex(".bag")] bag_out = rosbag.Bag(name + "-1700points_per_scan-4D.bag", 'w') for topic, msg, time in bag_in.read_messages(): '''if rospy.is_shutdown(): break''' '''if topic == "/tf": for tr in msg.transforms: tr.header.stamp = rospy.Time.now() pub_tf.publish(msg)''' if topic == "/camera/depth/points": point_cloud = msg pts = point_cloud2.pointcloud2_to_array(point_cloud) d = {} for raw in pts: for pt in raw: pt = np.array([pt[0], pt[1], pt[2], 1]) if isnan(pt[0]) or isnan(pt[1]) or isnan(pt[2]): continue cell = world_to_cell(pt, 0.05) if cell in d: d[cell] = np.append(d[cell], pt) else: d[cell] = np.array([pt]) float_array = np.array([]) '''marker_msg = Marker() marker_msg.header.stamp = rospy.Time.now() marker_msg.header.frame_id = point_cloud.header.frame_id
def lidar_callback(self, msg): pc_arr = point_cloud2.pointcloud2_to_array(msg) pc_arr = structured_to_unstructured(pc_arr) #print(pc_arr.shape) pc_arr = pc_arr.reshape(-1, 4) lidar_boxes, lidar_scores = self.model.predict(pc_arr) #print(lidar_boxes) # points.dtype=[('x', np.float32),('y', np.float32),('z', np.float32),('intensity', np.float32)] # cloud_msg=point_cloud2.array_to_pointcloud2(points,rospy.Time.now(),"rslidar") if lidar_boxes is not None: num_detects = len(lidar_boxes) #if len(lidar_boxes)<=10 else 10 arr_bbox = BoundingBoxArray() arr_score = MarkerArray() for i in range(num_detects): bbox = BoundingBox() bbox.header.frame_id = msg.header.frame_id bbox.header.stamp = rospy.Time.now() bbox.pose.position.x = float(lidar_boxes[i][0]) bbox.pose.position.y = float(lidar_boxes[i][1]) #bbox.pose.position.z = float(lidar_boxes[i][2]) bbox.pose.position.z = float( lidar_boxes[i][2]) + float(lidar_boxes[i][5]) / 2 bbox.dimensions.x = float(lidar_boxes[i][3]) # width bbox.dimensions.y = float(lidar_boxes[i][4]) # length bbox.dimensions.z = float(lidar_boxes[i][5]) # height q = Quaternion(axis=(0, 0, 1), radians=float(-lidar_boxes[i][6])) bbox.pose.orientation.x = q.x bbox.pose.orientation.y = q.y bbox.pose.orientation.z = q.z bbox.pose.orientation.w = q.w arr_bbox.boxes.append(bbox) marker = Marker() marker.header.frame_id = msg.header.frame_id marker.header.stamp = rospy.Time.now() marker.ns = "basic_shapes" marker.id = i marker.type = Marker.TEXT_VIEW_FACING marker.action = Marker.ADD marker.lifetime = rospy.Duration(0.15) marker.scale.x = 4 marker.scale.y = 4 marker.scale.z = 4 # Marker的颜色和透明度 marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1 marker.color.a = 1 marker.pose.position.x = float(lidar_boxes[i][0]) marker.pose.position.y = float(lidar_boxes[i][1]) marker.pose.position.z = float( lidar_boxes[i][2]) + float(lidar_boxes[i][5]) / 2 marker.text = str(np.around(lidar_scores[i], 2)) arr_score.markers.append(marker) arr_bbox.header.frame_id = msg.header.frame_id arr_bbox.header.stamp = rospy.Time.now() print("Number of detections: {}".format(num_detects)) self.pub_bbox.publish(arr_bbox) self.pub_text.publish(arr_score)
def pointcloud2_to_array_xyz(pc): points = pointcloud2_to_array(pc) xyzs = np.vstack((points['x'], points['y'], points['z'])).T return xyzs
def getVelodyne(self, msg): # Recebendo odometria teste = pc2.pointcloud2_to_array(msg) x = [] y = [] z = [] # Filtrando dados (pontos) para diminuir quantidade de processamento for i, j in enumerate(teste['x']): if i%5 == 0: x.append(j) for i, j in enumerate(teste['y']): if i%5 == 0: y.append(j) for i, j in enumerate(teste['z']): if i%5 == 0: z.append(j) # Transformando valores em um array teste_menor = np.array([(i, j, k) for i, j, k in zip(x, y, z)], teste.dtype) # Criando dataframe para armazenar os dados df = pd.DataFrame(teste_menor, columns=['x','y','z']) print('--------------------------------------------------------------------------------------') df = df[df['z'] > 0.2] # Definindo tolerancias para os filtros tolerance = 0.1 negative_tolerance = -0.1 # Filtrando menor distancia frontal self.frente = np.nan distancia_frontal = df[df['y'] < tolerance] distancia_frontal = distancia_frontal[distancia_frontal['y'] > negative_tolerance] distancia_frontal = distancia_frontal[distancia_frontal['x'] > negative_tolerance] print('Distancia frontal: ') if(distancia_frontal.isnull().values.all()): print('Nada a frente') self.frente = np.nan else: distancia_frontal.reset_index(drop=True, inplace=True) value = distancia_frontal[['x']].idxmin() distancia_frontal = distancia_frontal.iloc[value][:] self.frente = distancia_frontal.at[value[0],'x'] print(self.frente) print('') # Filtrando menor distancia da direita (90 graus) self.direita = np.nan distancia_direita = df[df['x'] < tolerance] distancia_direita = distancia_direita[distancia_direita['y'] < 0] distancia_direita = distancia_direita[distancia_direita['x'] > negative_tolerance] print('Distancia direita: ') if(distancia_direita.isnull().values.all()): print('Nada a direita') self.direita = np.nan else: distancia_direita.reset_index(drop=True, inplace=True) value = distancia_direita[['y']].idxmin() distancia_direita = distancia_direita.iloc[value][:] self.direita = distancia_direita.at[value[0],'y'] self.direita = abs(self.direita) print(self.direita) print('') # Filtrando menor distancia da esquerda (90 graus) self.esquerda = np.nan distancia_esquerda = df[df['x'] < tolerance] distancia_esquerda = distancia_esquerda[distancia_esquerda['y'] > 0] distancia_esquerda = distancia_esquerda[distancia_esquerda['x'] > negative_tolerance] print('Distancia esquerda: ') if(distancia_esquerda.isnull().values.all()): print('Nada a esquerda') self.esquerda = np.nan else: distancia_esquerda.reset_index(drop=True, inplace=True) value = distancia_esquerda[['y']].idxmin() distancia_esquerda = distancia_esquerda.iloc[value][:] self.esquerda = distancia_esquerda.at[value[0],'y'] print(self.esquerda) print('') # Filtrando distancia diagonal direita (45 graus) #self.s_direita = np.nan dd_direita = df.loc[((abs(df['x'] + df['y'])) <= (tolerance)) & df['x'] > 0] print('Distancia diagonal direita: ') if(dd_direita.isnull().values.all()): print('Nada na diagonal direita') #self.s_direita = np.nan else: dd_direita.reset_index(drop=True, inplace=True) value = dd_direita[['x']].idxmin() dd_direita = dd_direita.iloc[value][:] #print(dd_direita.at[value[0],'x']) #x = dd_direita.at[value[0],'x']**2 self.s_direita = dd_direita.at[value[0],'y'] self.s_direita = abs(self.s_direita) #soma = x + y #hip_direita = math.sqrt(soma) print(self.s_direita) print('') # Filtrando distancia diagonal esquerda (45 graus) #self.s_direita = np.nan dd_esquerda = df.loc[((abs(df['x'] - 2*df['y'])) <= (tolerance)) & df['x'] > 0] print('Distancia diagonal esquerda: ') if(dd_esquerda.isnull().values.all()): print('Nada na diagonal esquerda') #self.s_direita = np.nan else: dd_esquerda.reset_index(drop=True, inplace=True) value = dd_esquerda[['x']].idxmin() dd_esquerda = dd_esquerda.iloc[value][:] #print(dd_esquerda.at[value[0],'x']) #x = dd_esquerda.at[value[0],'x']**2 self.s_esquerda = dd_esquerda.at[value[0],'y'] #soma = x + y #hip_esquerda = math.sqrt(soma) #print(hip_esquerda) print(self.s_esquerda) print('')