def arr_to_fields(cloud_arr): '''Convert a numpy record datatype into a list of PointFields. ''' fields = [] for field_name in cloud_arr.dtype.names: np_field_type, field_offset = cloud_arr.dtype.fields[field_name] pf = PointField() pf.name = field_name pf.datatype = nptype_to_pftype[np_field_type] pf.offset = field_offset pf.count = 1 # is this ever more than one? fields.append(pf) return fields
def point_cloud_2(cls, msg, obj): obj.header = decode.header(msg, obj.header, "") obj.height = msg["height"] obj.width = msg["width"] for i in range(msg['_length']): pf = PointField() pf.name = msg['%s_name' % i] pf.offset = msg['%s_offset' % i] pf.datatype = msg['%s_datatype' % i] pf.count = msg['%s_count' % i] obj.fields.append(pf) obj.is_bigedian = msg['is_bigedian'] obj.point_step = msg['point_step'] obj.row_step = msg['row_step'] obj.data = msg['data'] obj.is_dense = msg['is_dense'] return(obj)
def xyzs2pc2(xyzs,stamp,seq,frame_id): pc2Data = PointCloud2() if len(xyzs) != 0: if type(xyzs[0][0]) == np.float64: xyzs = np.array(xyzs,dtype=np.float32) pc2Data.is_dense = False pc2Data.is_bigendian = False pc2Data.height = 1 pc2Data.width = len(xyzs) pc2Data.header.frame_id = frame_id pc2Data.header.seq = seq pc2Data.header.stamp = stamp; for i in range(3): pc2Field = PointField() if i == 0: pc2Field.name = 'x' elif i == 1: pc2Field.name = 'y' elif i == 2: pc2Field.name = 'z' else: pc2Field.name = 'unknown' pc2Field.count = 1 #if type(xyzs[0][0]) == np.float64: # pc2Field.datatype = 8 # FLOAT64 # dataLen = 8 # FLOAT64 if type(xyzs[0][0]) == np.float32: pc2Field.datatype = 7 # FLOAT32 dataLen = 4 # FLOAT32 else: assert False, "unknown datatype" pc2Field.offset = i*dataLen pc2Data.fields.append(pc2Field) pc2Data.point_step = i*dataLen + dataLen; pc2Data.row_step = pc2Data.point_step * pc2Data.width; data_buf = xyzs.flatten().tostring() assert len(data_buf)/float(dataLen)/3.0 == len(xyzs), "len(data_buf)/4.0/3.0 = "+np.str(len(data_buf)/4.0/3.0)+" but len(xyzs) = "+np.str(len(xyzs)) pc2Data.data = data_buf return pc2Data
def dtype_to_fields(dtype): '''Convert a numpy record datatype into a list of PointFields. ''' fields = [] for field_name in dtype.names: np_field_type, field_offset = dtype.fields[field_name] pf = PointField() pf.name = field_name if np_field_type.subdtype: item_dtype, shape = np_field_type.subdtype pf.count = np.prod(shape) np_field_type = item_dtype else: pf.count = 1 pf.datatype = nptype_to_pftype[np_field_type] pf.offset = field_offset fields.append(pf) return fields
def pointCloud2Show(): rospy.init_node('point_cloud_publisher',anonymous = True) pub = rospy.Publisher('/cloud',PointCloud2,queue_size = 50) #rospy.init_node('point_cloud_publisher',anonymous = True) rate = rospy.Rate(10) clouds_old = csv.reader(open('/home/cheney/LearnLog/ROS/hello_rospy/src/beginner_tutorials/scripts/laser.csv','r')) clouds = [] for cloud in clouds_old: cloud = map(float,cloud) clouds.append(cloud) num_points = len(clouds) while not rospy.is_shutdown(): fields = [] x = PointField() x.datatype = 7 x.name = 'x' x.count = 1 fields.append(x) y = PointField() y.datatype = 7 y.name = 'y' y.count = 1 fields.append(y) z = PointField() z.datatype = 7 z.name = 'z' z.count = 1 fields.append(z) inte = PointField() inte.datatype = 7 inte.name = 'intensity' inte.count = 1 fields.append(inte) points = [] for i in range(num_points): alpha = (clouds[i][0]/180.0) * math.pi lamda = (clouds[i][1]/180.0) * math.pi distance = clouds[i][2] intensity = clouds[i][3] #not for sure if coordinate is right zz = distance * math.sin(lamda) yy = distance * math.cos(lamda) * math.cos(alpha) xx = distance * math.cos(lamda) * math.cos(alpha) points.append([xx,yy,zz,intensity]) header = Header() header.stamp = rospy.Time.now() header.frame_id = 'sensor_frame' cloud2 = point_cloud2.create_cloud(header , fields , points) pub.publish(cloud2) rate.sleep() print 'done'
def pcl_callback(self, pcl): """Callback for the pcl. Input should be a PointCloud2.""" self.rate_counter += 1 if self.rate_counter % 3 == 0: print 'Received Pointcloud. ' + str(time.time()) points_as_list = pc2_to_list(pcl) # Convert to List thresholded = self.thresholding(points_as_list) # Apply Thresholds # Uniform reduction uniform_reduction_factor = 1 thresholded = list( thresholded[i] for i in xrange(0, len(thresholded), uniform_reduction_factor)) # DEBUG Test publish thresholded points 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" cloud = pc2.create_cloud(header, fields, thresholded) cloud.header.stamp = rospy.Time.now() self.threshold_publisher.publish(cloud) # END Test publish if self.started: if self.pcl_reference == None: self.pcl_reference = o3d.geometry.PointCloud() # pylint: disable=no-member self.pcl_reference.points = o3d.utility.Vector3dVector( thresholded) # pylint: disable=no-member self.pcl_reference.estimate_normals( fast_normal_computation=False) # Store reference point position pcd_tree = o3d.geometry.KDTreeFlann(self.pcl_reference) # pylint: disable=no-member ref_point = None ref_point_nb = 0 # print len(self.pcl_reference.points) for i in range(0, len(self.pcl_reference.points)): nb = pcd_tree.search_radius_vector_3d( self.pcl_reference.points[i], 0.01) if nb > ref_point_nb: ref_point = self.pcl_reference.points[i] ref_point_nb = nb # print i self.position_reference = np.asarray( [[1.0, 0, 0, ref_point[0]], [0, 1.0, 0, ref_point[1]], [0, 0, 1.0, ref_point[2]], [0.0, 0.0, 0.0, 1.0]]) # print(self.position_reference) position_shaped = self.position_reference.reshape((16)) self.position_publisher.publish(position_shaped.tolist()) # self.started = False else: calctime = time.time() # Calculate diff from ICP head_tf = self.calc_head_transform_icp(thresholded) #print np.round(head_tf, decimals=4) # Calculate position # if not self.head_pos == None: # DEBUG PRINT # print (np.dot(self.position_reference, np.linalg.inv(head_tf)) - self.head_pos) #self.head_pos = np.dot(self.position_reference, np.linalg.inv(head_tf)) self.head_pos = np.dot(self.position_reference, head_tf) #print 'Calculation finished ' + str((time.time() - calctime)) print np.round(self.head_pos, decimals=4) #print "Reference:" #print np.round(self.position_reference, decimals=4) head_tf_shaped = self.head_pos.reshape((16)) # Reshape self.position_publisher.publish( head_tf_shaped.tolist()) # Publish # Eval self.calculation_times.append((time.time() - calctime)) self.positions.append(self.head_pos) self.ur5_pos_a.append(self.last_ur5_pos)
def convert_numpy_2_pointcloud2_color(points, stamp=None, frame_id=None, maxDistColor=None): ''' Create a sensor_msgs.PointCloud2 from an array of points. This function will automatically assign RGB values to each point. The RGB values are determined by the distance of a point from the origin. Use maxDistColor to set the distance at which the color corresponds to the farthest distance is used. points: A NumPy array of Nx3. stamp: An alternative ROS header stamp. frame_id: The frame id. String. maxDisColor: Should be positive if specified.. This function get inspired by https://github.com/spillai/pybot/blob/master/pybot/externals/ros/pointclouds.py https://gist.github.com/lucasw/ea04dcd65bc944daea07612314d114bb (https://answers.ros.org/question/289576/understanding-the-bytes-in-a-pcl2-message/) and expo_utility.xyz_array_to_point_cloud_msg() function of the AirSim package. ROS sensor_msgs/PointField. http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointField.html More references on mixed-type NumPy array, structured array. https://docs.scipy.org/doc/numpy/reference/arrays.dtypes.html https://stackoverflow.com/questions/37791134/merge-width-x-height-x-3-numpy-uint8-array-into-width-x-height-x-1-uint32-array https://jakevdp.github.io/PythonDataScienceHandbook/02.09-structured-data-numpy.html ''' # Clipping input. dist = np.linalg.norm(points, axis=1) if (maxDistColor is not None and maxDistColor > 0): dist = np.clip(dist, 0, maxDistColor) # Compose color. cr, cg, cb = color_map(dist, DIST_COLORS, DIST_COLOR_LEVELS) C = np.zeros((cr.size, 4), dtype=np.uint8) + 255 C[:, 0] = cb.astype(np.uint8) C[:, 1] = cg.astype(np.uint8) C[:, 2] = cr.astype(np.uint8) C = C.view("uint32") # Structured array. pointsColor = np.zeros( (points.shape[0], 1), \ dtype={ "names": ( "x", "y", "z", "rgba" ), "formats": ( "f4", "f4", "f4", "u4" )} ) points = points.astype(np.float32) pointsColor["x"] = points[:, 0].reshape((-1, 1)) pointsColor["y"] = points[:, 1].reshape((-1, 1)) pointsColor["z"] = points[:, 2].reshape((-1, 1)) pointsColor["rgba"] = C header = Header() if stamp is None: header.stamp = rospy.Time().now() else: header.stamp = stamp if frame_id is None: header.frame_id = "None" else: header.frame_id = frame_id msg = PointCloud2() msg.header = header if len(points.shape) == 3: msg.height = points.shape[1] msg.width = points.shape[0] else: msg.height = 1 msg.width = points.shape[0] msg.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), ] msg.is_bigendian = False msg.point_step = 16 msg.row_step = msg.point_step * points.shape[0] msg.is_dense = int(np.isfinite(points).all()) msg.data = pointsColor.tostring() return msg
z_min = np.min(boxes_corner[:, 2]) car_labels = np.logical_or( car_labels, bounding_box(points, x_min, x_max, y_min, y_max, z_min, z_max)) for i, label in enumerate(car_labels): if label: points[i, 3] = 100 # f2 = plt.figure(figsize=(15, 8)) # ax2 = f2.add_subplot(111, projection='3d') # draw_point_cloud_color(ax2, 'Velodyne scan', points, car_colors, xlim3d=(-10,30)) # plt.show() #header header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'map' #create pcl from 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('rgba', 12, PointField.UINT32, 1), ] scaled_polygon_pcl = pcl2.create_cloud(header, fields, points) pcl_pub.publish(scaled_polygon_pcl) rate.sleep()
def setup_pc2msg(): msg = PointCloud2() f1 = PointField() f2 = PointField() f3 = PointField() f4 = PointField() #msg.header.frame_id = "usb_cam" msg.height = 1 #msg.width = 3 msg.point_step = 20 #msg.row_step = 30 f1.name = "x" f1.offset = 0 f1.datatype = 7 f1.count = 1 f2.name = "y" f2.offset = 4 f2.datatype = 7 f2.count = 1 f3.name = "z" f3.offset = 8 f3.datatype = 7 f3.count = 1 f4.name = "rgb" f4.offset = 16 f4.datatype = 7 f4.count = 1 msg.fields = [f1, f2, f3, f4] msg.is_dense = False return msg
def newSegmentationReceived(self, laserscan, laserscanSegmentation): pointCount = len(laserscan.ranges) labelsOfPoint = [] cartesianCoordinates = [] centroidsOfLabel = dict() numIgnoredPoints = 0 for pointIndex in xrange(0, pointCount): labelsOfPoint.append(set()) cartesianCoordinates.append( self.calculateCartesianCoordinates(laserscan, pointIndex)) numIgnoredPoints += 1 if laserscan.ranges[ pointIndex] < self.minDistanceToSensor else 0 for segment in laserscanSegmentation.segments: centroid = numpy.array([0.0, 0.0, 0.0]) for pointIndex in segment.measurement_indices: labelsOfPoint[pointIndex].add(segment.label) centroid += cartesianCoordinates[pointIndex] centroid /= float(len(segment.measurement_indices)) centroidsOfLabel[segment.label] = centroid header = laserscan.header cloud = PointCloud2(header=header, height=1, width=pointCount - numIgnoredPoints, point_step=16, row_step=16 * (pointCount - numIgnoredPoints)) # or step 32? cloud.fields.append( PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1)) cloud.fields.append( PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1)) cloud.fields.append( PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1)) cloud.fields.append( PointField(name="rgb", offset=12, datatype=PointField.FLOAT32, count=1)) # or offset 16? markerArray = MarkerArray() for pointIndex in xrange(0, pointCount): # Skip wrong echoes very close to sensor if laserscan.ranges[pointIndex] < self.minDistanceToSensor: continue # If one point has multiple labels, we just linearly interpolate between the corresponding label colors colors = [] for label in labelsOfPoint[pointIndex]: colors.append(self.lookupColorForLabel(label)) if colors: resultingColor = numpy.array([0.0, 0.0, 0.0]) for color in colors: resultingColor += color resultingColor /= float(len(colors)) else: resultingColor = numpy.array(self.unlabelledColor) # Add points to point cloud cloud.data += struct.pack('f', cartesianCoordinates[pointIndex][0]) # x cloud.data += struct.pack('f', cartesianCoordinates[pointIndex][1]) # y cloud.data += struct.pack('f', cartesianCoordinates[pointIndex][2]) # z cloud.data += chr(int(resultingColor[2] * 255)) # r cloud.data += chr(int(resultingColor[1] * 255)) # g cloud.data += chr(int(resultingColor[0] * 255)) # b cloud.data += chr(0) # a # Add text markers to marker array for segment in laserscanSegmentation.segments: centroid = centroidsOfLabel[segment.label] color = self.lookupColorForLabel(segment.label) # Ignore segments very close to sensor, caused by wrong echoes due to robot self-reflection etc. if math.hypot(centroid[0], centroid[1]) < self.minDistanceToSensor: continue textMarker = Marker(header=header) textMarker.type = Marker.TEXT_VIEW_FACING textMarker.id = len(markerArray.markers) textMarker.text = "%d" % segment.label textMarker.color = ColorRGBA(r=color[0], g=color[1], b=color[2], a=1) textMarker.scale.z = 0.6 * self.fontScale textMarker.pose.position.x = centroid[0] + 0.4 # for readability textMarker.pose.position.y = centroid[1] textMarker.pose.position.z = centroid[2] markerArray.markers.append(textMarker) # Delete old markers which are not needed any more currentMarkerCount = len( markerArray.markers) # must be before creating delete markers for markerId in xrange(len(markerArray.markers), self._lastMarkerCount): deleteMarker = Marker(header=header) deleteMarker.id = markerId deleteMarker.action = Marker.DELETE markerArray.markers.append(deleteMarker) self._lastMarkerCount = currentMarkerCount self.markerArrayPublisher.publish(markerArray) self.cloudPublisher.publish(cloud)
def __init__(self, pybullet, robot, **kargs): # get "import pybullet as pb" and store in self.pb self.pb = pybullet # get robot from parent class self.robot = robot # create image msg placeholder for publication self.image_msg = Image() # get RGBD camera parameters from ROS param server self.image_msg.width = rospy.get_param('~rgbd_camera/resolution/width', 640) self.image_msg.height = rospy.get_param( '~rgbd_camera/resolution/height', 480) assert (self.image_msg.width > 5) assert (self.image_msg.height > 5) cam_frame_id = rospy.get_param('~rgbd_camera/frame_id', None) if not cam_frame_id: rospy.logerr( 'Required parameter rgbd_camera/frame_id not set, will exit now...' ) rospy.signal_shutdown( 'Required parameter rgbd_camera/frame_id not set') return # get pybullet camera link id from its name link_names_to_ids_dic = kargs['link_ids'] if not cam_frame_id in link_names_to_ids_dic: rospy.logerr( 'Camera reference frame "{}" not found in URDF model'.format( cam_frame_id)) rospy.logwarn( 'Available frames are: {}'.format(link_names_to_ids_dic)) rospy.signal_shutdown( 'required param rgbd_camera/frame_id not set properly') return self.pb_camera_link_id = link_names_to_ids_dic[cam_frame_id] self.image_msg.header.frame_id = cam_frame_id # create publisher self.pub_image = rospy.Publisher('rgb_image', Image, queue_size=1) self.image_msg.encoding = rospy.get_param( '~rgbd_camera/resolution/encoding', 'rgb8') self.image_msg.is_bigendian = rospy.get_param( '~rgbd_camera/resolution/encoding', 0) self.image_msg.step = rospy.get_param( '~rgbd_camera/resolution/encoding', 1920) # projection matrix self.hfov = rospy.get_param('~rgbd_camera/hfov', 56.3) self.vfov = rospy.get_param('~rgbd_camera/vfov', 43.7) self.near_plane = rospy.get_param('~rgbd_camera/near_plane', 0.4) self.far_plane = rospy.get_param('~rgbd_camera/far_plane', 8) self.projection_matrix = self.compute_projection_matrix() # use cv_bridge ros to convert cv matrix to ros format self.image_bridge = CvBridge() # variable used to run this plugin at a lower frequency, HACK self.count = 0 ## Setting TF static transform from camera to point cloud data in required direction point_cloud_frame = "point_cloud_camera" broadcaster = tf2_ros.StaticTransformBroadcaster() static_transformStamped = geometry_msgs.msg.TransformStamped() static_transformStamped.header.stamp = rospy.Time.now() static_transformStamped.header.frame_id = cam_frame_id static_transformStamped.child_frame_id = point_cloud_frame static_transformStamped.transform.translation.x = 0 static_transformStamped.transform.translation.y = 0 static_transformStamped.transform.translation.z = 0 quat = tf.transformations.quaternion_from_euler(np.deg2rad(-90), 0, 0) static_transformStamped.transform.rotation.x = quat[0] static_transformStamped.transform.rotation.y = quat[1] static_transformStamped.transform.rotation.z = quat[2] static_transformStamped.transform.rotation.w = quat[3] broadcaster.sendTransform(static_transformStamped) # publisher for depth image self.pub_depth_image = rospy.Publisher('depth_image', Image, queue_size=1) # image msg for depth image self.depth_image_msg = Image() self.depth_image_msg.width = rospy.get_param( '~rgbd_camera/resolution/width', 640) self.depth_image_msg.height = rospy.get_param( '~rgbd_camera/resolution/height', 480) self.depth_image_msg.encoding = rospy.get_param( '~rgbd_camera/resolution/encoding', '32FC1') self.depth_image_msg.is_bigendian = rospy.get_param( '~rgbd_camera/resolution/encoding', 0) self.depth_image_msg.step = rospy.get_param( '~rgbd_camera/resolution/encoding', 2560) # publisher for point_cloud self.pub_point_cloud = rospy.Publisher('point_cloud', PointCloud2, queue_size=1) # point cloud msg self.point_cloud_msg = PointCloud2() self.point_cloud_msg.header.frame_id = point_cloud_frame ## The new frame is the static frame self.point_cloud_msg.width = rospy.get_param( '~rgbd_camera/resolution/width', 640) self.point_cloud_msg.height = rospy.get_param( '~rgbd_camera/resolution/height', 480) self.point_cloud_msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] self.point_cloud_msg.is_bigendian = rospy.get_param( '~rgbd_camera/resolution/encoding', 0) self.point_cloud_msg.point_step = 12 self.point_cloud_msg.row_step = self.point_cloud_msg.width * self.point_cloud_msg.point_step self.point_cloud_msg.is_dense = True # organised point cloud
def callback(self, image_msg): """ Convert img_msg into point cloud with color mappings via numpy. """ begin_time = time.time() rospy.logdebug("Received new image, seq %d at %f" % (image_msg.header.seq, image_msg.header.stamp.to_sec())) header = Header() header = image_msg.header frame_id = rospy.get_param("~frame_id", None) if frame_id: header.frame_id = frame_id fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('r', 12, PointField.FLOAT32, 1), PointField('g', 16, PointField.FLOAT32, 1), PointField('b', 20, PointField.FLOAT32, 1), PointField('a', 24, PointField.FLOAT32, 1) ] nranges = len(image_msg.ranges) nangles = len(image_msg.azimuth_angles) npts = nranges * nangles if self.geometry is None: self.make_geometry(image_msg) if self.color_lookup is None: self.make_color_lookup() t0 = time.time() intensities = self.process_intensity_array(image_msg.intensities, image_msg.data_size) # Expand out intensity array (for fast comparison) expanded_intensities = np.repeat(intensities[..., np.newaxis], 4, axis=1) # Fill the output array for i in range(len(self.elevations)): points = np.empty((nranges * nangles, 7)) if points[:, 0:3].shape != self.geometry[i, :, :].shape: # Occassionally the sonar message has a changing geometry # that really screws stuff up until it's resolved. Fix it here rospy.logdebug('Change in image size! Remaking geometry...') self.make_geometry(image_msg) points[:, 0:3] = self.geometry[i, :, :] points[:, 3:] = np.where( expanded_intensities > self.intensity_threshold, self.color_lookup[expanded_intensities[:, 0]], np.zeros((nranges * nangles, 4))) step = i * (nranges * nangles) next_step = step + (nranges * nangles) self.output_points[step:next_step, :] = points t1 = time.time() N = len(self.output_points) cloud_msg = PointCloud2(header=header, height=1, width=N, is_dense=True, is_bigendian=False, fields=fields, point_step=28, row_step=28 * N, data=self.output_points.tostring()) dt1 = time.time() - t1 dt0 = t1 - t0 total_time = time.time() self.pub.publish(cloud_msg) rospy.loginfo( f"published pointcloud: npts = {npts}, Find Pts = {dt0:0.5f} sec, Convert to Cloud = {dt1:0.5f} sec. Total Time = {(total_time - begin_time):0.3f} sec" )
def process(): # pub = rospy.Publisher('velodyne_point_data', String, queue_size=10) # while not rospy.is_shutdown(): rospy.init_node('test_velodyne', anonymous=True) bag_name = "kitti_2011_09_26_drive_0001_synced" bag_dir = "/home/cuberick/raw_data/%s.bag" % (bag_name) bag = rosbag.Bag(bag_dir) interval = 1 density = 4 duration = rospy.Duration(0.1, 0) # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # Read IMU-to-Velodyne Transformation Matrix tcount = 1 print("===============================================") print("| ---PROGRAM START--- |") print("| |") print("| Welcome to SWORD ART ONLINE |") print("| Powered by The SEED |") print("| |") print("| |") print("| Ver. RC |") print("| by Cuberick.YuukiAsuna |") print("===============================================") print print sys.stdout.write("KITTI sequence: %s" % bag_name) print sys.stdout.write("Frame interval: %d, Point density: %d" % (interval, density)) # sys.stdout.flush() print print print("Bag LOADED, NERvGear START") print sys.stdout.write("\r>>>Read tf info") sys.stdout.flush() # print for topic, msg, t in bag.read_messages("/tf_static"): # if tcount < 1: # break # tcount -= 1 # print count All_tfs = msg.transforms # Extract T_imu_to_velo for transf in All_tfs: if transf.child_frame_id == "velo_link": T_imu_to_velo = transf.transform # Transform between quaternion and Euler T_imu_to_velo_Quaternion_rotation = T_imu_to_velo.rotation T_imu_to_velo_translation = T_imu_to_velo.translation # print(T_imu_to_velo_Quaternion_rotation) quaternion = (T_imu_to_velo_Quaternion_rotation.x, T_imu_to_velo_Quaternion_rotation.y, T_imu_to_velo_Quaternion_rotation.z, T_imu_to_velo_Quaternion_rotation.w) T_imu_to_velo_Euler_rotaiton = tf.transformations.euler_from_quaternion( quaternion) # print(T_imu_to_velo_Euler_rotaiton) roll = T_imu_to_velo_Euler_rotaiton[0] pitch = T_imu_to_velo_Euler_rotaiton[1] yaw = T_imu_to_velo_Euler_rotaiton[2] T_imu_to_velo_homo = np.empty([4, 4], dtype=float) T_imu_to_velo_homo = [ [ math.cos(yaw) * math.cos(pitch), -math.cos(yaw) * math.sin(pitch) * math.sin(roll) + math.sin(yaw) * math.cos(roll), -math.cos(yaw) * math.sin(pitch) * math.cos(roll) - math.sin(yaw) * math.sin(roll), -T_imu_to_velo_translation.x ], [ -math.sin(yaw) * math.cos(pitch), math.sin(yaw) * math.sin(pitch) * math.sin(roll) + math.cos(yaw) * math.cos(roll), -math.sin(yaw) * math.sin(pitch) * math.cos(roll) + math.cos(yaw) * math.sin(roll), -T_imu_to_velo_translation.y ], [ math.sin(pitch), -math.cos(pitch) * math.sin(roll), math.cos(pitch) * math.cos(roll), -T_imu_to_velo_translation.z ], [0, 0, 0, 1] ] # print T_imu_to_velo_homo sys.stdout.write("\r T_imu_to_velo obtained") sys.stdout.flush() # print (" T_imu_to_velo obtained") # print # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # Read OXTS data OXTS_GPS_raw = np.empty([1, 3], dtype=float) gcount = 1 sys.stdout.write("\r>>>Read OXTS GPS raw data") sys.stdout.flush() # print for topic, msg, t in bag.read_messages("/kitti/oxts/gps/fix"): # if gcount < 1: # break # gcount -= 1 current_GPS_data = [msg.latitude, msg.longitude, msg.altitude] OXTS_GPS_raw = np.vstack([OXTS_GPS_raw, current_GPS_data]) OXTS_GPS_raw = np.delete(OXTS_GPS_raw, (0), axis=0) sys.stdout.write("\r OSTX GPS raw data obtained") # print # print(OXTS_GPS_raw) sys.stdout.write("\r>>>Read OXTS IMU data") sys.stdout.flush() # print OXTS_IMU_raw = np.empty([1, 3], dtype=float) icount = 3 for topic, msg, t in bag.read_messages("/kitti/oxts/imu"): # if icount < 1: # break # icount -= 1 # print msg IMU_raw = msg.orientation quaternion_IMU = (IMU_raw.x, IMU_raw.y, IMU_raw.z, IMU_raw.w) IMU_data = tf.transformations.euler_from_quaternion(quaternion_IMU) IMU_roll = IMU_data[0] IMU_pitch = IMU_data[1] IMU_heading = IMU_data[2] OXTS_IMU_raw = np.vstack( [OXTS_IMU_raw, [IMU_roll, IMU_pitch, IMU_heading]]) # print IMU_data OXTS_IMU_raw = np.delete(OXTS_IMU_raw, (0), axis=0) # print OXTS_IMU_raw sys.stdout.write("\r OXTS_IMU data obtained") sys.stdout.flush() # print # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # ---->>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # get pose (loadOxtsliteData and convertOxtsToPose) # compute scale from first lat value oxts_first = OXTS_GPS_raw[0][0] scale = math.cos(oxts_first * math.pi / 180.00) # print scale # OXTS_GPS_raw [1] [2] [3] and OXTS_IMU_raw [1] [2] [3] oxts = np.concatenate((OXTS_GPS_raw, OXTS_IMU_raw), axis=1) # print oxts lengh_of_oxts = np.shape(oxts)[0] # print lengh_of_oxts pose = [None] * lengh_of_oxts Tr_0_inv = np.zeros(shape=(4, 4)) isempty = np.zeros(shape=(4, 4)) # a = oxts[0] # print(a) i = 0 for i in range(lengh_of_oxts - 1): if oxts[i] == []: pose[i] = np.empty(( 3, 3, )) * np.nan continue t = np.empty(( 3, 1, )) current_oxts_1 = oxts[i][0] current_oxts_2 = oxts[i][1] er = 6378137 current_t_11 = scale * current_oxts_2 * math.pi * er / 180 current_t_12 = scale * er * math.log( math.tan((90 + current_oxts_1) * math.pi / 360)) current_t_13 = oxts[i][2] t = [[0], [0], [0]] # t = [[current_t_11], [current_t_12], [current_t_13]] # print t # print # print i # print(oxts[i]) rx = oxts[i][3] ry = oxts[i][4] rz = oxts[i][5] # print (rx) # print (ry) # print (rz) Rx = np.matrix([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]]) Ry = np.matrix([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]]) Rz = np.matrix([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]]) R = np.empty(( 3, 3, )) R = np.dot(np.dot(Rz, Ry), Rx) # print (Rx) # print (Ry) # print (Rz) # print R # print current_matrix = np.zeros(shape=(4, 4)) first_three_row = np.concatenate((R, t), axis=1) current_matrix = np.vstack([first_three_row, [0, 0, 0, 1]]) # print first_three_row if np.array_equal(Tr_0_inv, isempty): # print "enter if statement" # print i Tr_0_inv = inv(current_matrix) # if i == 0: # print Tr_0_inv # print four_rows current_pose = np.empty(( 4, 4, )) current_pose = Tr_0_inv.dot(current_matrix) pose[i] = current_pose # print i # print oxts[i] # print pose[i] # raw_input("press ehnter to continue") # ---->>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> pose_T = [None] * lengh_of_oxts for i in range(lengh_of_oxts - 1): transfer_pose = np.empty(( 4, 4, )) # print (T_imu_to_velo_homo) # print # print(pose[i]) transfer_pose = np.dot(T_imu_to_velo_homo, pose[i]) pose_T[i] = np.empty(( 4, 4, )) pose_T[i] = transfer_pose frame = 0 frame_count = 0 frame_counts = 0 total_frames = 0 frames_left = 0 skipped_count = 0 rejected_count = 0 # for frame in range(0,interval,len(pose_T)): # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # Read velodyne info sys.stdout.write("\r>>>Read Velodyne point data") sys.stdout.flush() # print # all_points = np.empty([1,3],dtype=float) current_point_set = np.empty(( 999999, 3, )) * np.NaN vcount = 5 bag_count = -1 total_msg_no = 0 for topic, msg, t in bag.read_messages("/kitti/velo/pointcloud"): bag_count += 1 if (bag_count) % interval != 0: continue total_msg_no += 1 all_points = [np.empty([1, 4], dtype=float)] * total_msg_no bag_count = -1 for topic, msg, t in bag.read_messages("/kitti/velo/pointcloud"): # transformed_points = np.empty((1,3,)) transformed_points = np.empty(( 999999, 3, )) * np.NaN bag_count += 1 if (bag_count) % interval != 0: continue # if vcount < 1: # break # vcount -= 1 # print("counting cycles") # print vcount frame_count += 1 total_frames = len(pose_T) / interval total_frames = math.ceil(total_frames) frames_left = total_frames - frame_count + 1 info_of_frame = "Processing scan No.%d , %d remaining" % (frame_count, frames_left) sys.stdout.write("\r%s" % info_of_frame) sys.stdout.flush() # sys.stdout.write(" ~~~~~~working hard >.< please wait!~~~~~~~") # print # print vcount data_length = len(msg.data) ## msg is of type PointCloud2 raw_data = pc2.read_points(msg) point_count_raw = 0 for point in raw_data: current_point = [point[0], point[1], point[2]] # if point[0] > 4: try: # print point_count_raw # print current_point current_point_set[point_count_raw] = current_point point_count_raw += 1 except: # print ".^. skip recording this point" skipped_count += 1 continue current_point_set = np.delete(current_point_set, (0), axis=0) current_point_set = current_point_set[~np.isnan(current_point_set).any( axis=1)] velo = current_point_set break if np.shape(velo)[0] < 2: continue j = 0 point_count = -1 for j in range(np.shape(velo)[0]): # try:/ point_count += 1 if (point_count + 1) % density != 0: continue pose_a = pose_T[bag_count] # print pose_a point = velo[j] # print point a = type(point) # print a # print point point_a = point[np.newaxis, :].T # print point_a point_b = np.vstack([point_a, [1]]) # print point_b # print j # print (pose_a) # print (point_b) # print point_c = np.dot(pose_a, point_b) point_c = point_c[np.newaxis, :].T # print point_c point_c = np.delete(point_c, [3], axis=1) # print point_c # a = type(point_c) # print a # print point_c # print transformed_points[j] # raw_input("press ehnter to continue") if (point_c[0, 2] > -6) and (point_c[0, 2] < 6): transformed_points[j] = point_c else: rejected_count += 1 # print " O_o point rejected due to range limit" # except: # print "except!!!" # continue # print "Cumulated rejected and skipped points:" # print (rejected_count + skipped_count) # print # transformed_points = np.delete(transformed_points, [3], axis=1) transformed_points = transformed_points[~np.isnan(transformed_points). any(axis=1)] transformed_points = np.delete(transformed_points, (0), axis=0) all_points[frame_count - 1] = transformed_points all_points[frame_count - 1] = np.delete(all_points[frame_count - 1], (0), axis=0) # all_points = np.vstack([all_points, transformed_points]) # all_points = np.delete(all_points, (0), axis=0) # print(all_points) # a = all_points.shape # print(a) # print frame_count sys.stdout.write("\rVelodyne point data processing finished") sys.stdout.flush() # print # all_points = np.delete(all_points, (0), axis=0) # print all_points # print all_points # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # ---->>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # init pose # pose = # subprocess.call(['spd-say','start publishing']) # print all_points # print # print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>><<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<") # prsint(">>>>>>>>>>>>>>>>>>>>>>>>>>>>><<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<") sys.stdout.write("\rProcessing completed, generating system report") # print # sys.stdout.flush() # print a = type(all_points) b = np.shape(all_points) # print a sys.stdout.write(" Total frames:") print b[0] sys.stdout.write(" Skipped points:") print skipped_count sys.stdout.write(" Rejected points:") print rejected_count print # print # print ("Start visualising...") pcl_pub = rospy.Publisher("/velodyne_pub", PointCloud2, queue_size=10) rospy.loginfo("Publisher started at: /velodyne_pub") rospy.sleep(1.) rospy.loginfo("Publishing...") print print print bag.close() while (1): raw_input("\rNERvGear ... waiting for instruction") # sys.stdout.write("Start visualising...") sys.stdout.flush() k = 0 for k in range(total_msg_no): sys.stdout.write("\rVisualising frame %d" % k) sys.stdout.flush() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'map' 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) ] current_visual_set = all_points[k] current_visual_set = current_visual_set.tolist() # print all_points processed_data = pc2.create_cloud_xyz32(header, current_visual_set) rospy.sleep(duration) # [[1, 1, 1]] # a = [[1, 1, 1]] # b = type(a) # print b pcl_pub.publish(processed_data) sys.stdout.write("\rVisualisation complete") sys.stdout.flush()
#fig = pyplot.figure() #ax = Axes3D(fig) #ax.scatter(coords_cam[0][::100], coords_cam[1][::100], coords_cam[2][::100]) #pyplot.show() # Add color to point cloud for Rviz (does not work!!) red = np.left_shift(arr_rgb[:, :, 0].reshape(-1), 16) green = np.left_shift(arr_rgb[:, :, 1].reshape(-1), 8) blue = arr_rgb[:, :, 2].reshape(-1) coords_cam[-1] = red + green + blue # Create point cloud 2 message fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('rgb', 16, PointField.FLOAT32, 1) ] h = msg.Header() h.stamp = rospy.Time.now() h.frame_id = '/affordance_net' pc2_msg = pc2.create_cloud(h, fields, coords_cam.T) # Create an action message and populate its fields action_list = action_msg() action_list.header.frame_id = "camera_depth_optical_frame" for action in plan: action_list.action.append(action[0])
def callback(self, data): if (TIME_PROFILING): start = time.time() # t1 = rospy.get_time() camera_cx = data.P[2] camera_cy = data.P[6] camera_fx = data.P[0] camera_fy = data.P[5] camera_scale = 1000.0 cv_image = self.bridge.imgmsg_to_cv2(data.image, desired_encoding="passthrough") cv_depth = self.bridge.imgmsg_to_cv2(data.depth, desired_encoding="passthrough") height = cv_depth.shape[0] weight = cv_depth.shape[1] # t2 = rospy.get_time() # print("Get Msg: " + str(t2-t1) + "s") # t1 = rospy.get_time() buffer = [] for row in range(height): #遍历高 for col in range(weight): #遍历宽 # t11 = rospy.get_time() d = cv_depth[row, col] Luma = cv_image[row, col] # t22 = rospy.get_time() # print(" read depth: " + str(t22-t11) + "s") # t11 = rospy.get_time() if d == 0: continue z = d / camera_scale x = (col - camera_cx) * z / camera_fx y = (row - camera_cy) * z / camera_fy # t22 = rospy.get_time() # print(" compute point position: " + str(t22-t11) + "s") # t11 = rospy.get_time() if abs(y) > 0.1: continue buffer.append(struct.pack('ffff', x, y, z, Luma)) # t22 = rospy.get_time() # print(" append buffer: " + str(t22-t11) + "s") # point = kdl.Frame(kdl.Vector(x, y, z)) # point_global = posemath.fromMsg( data.pose ) * point # buffer.append(struct.pack('ffff', point_global.p.x(), point_global.p.y(), point_global.p.z(), Luma)) # t2 = rospy.get_time() # print("Img to Pointcloud buffer: " + str(t2-t1) + "s") # t1 = rospy.get_time() ros_msg = PointCloud2() ros_msg.header = data.header # ros_msg.header.frame_id = "odom" print(ros_msg.header) ros_msg.height = 1 ros_msg.width = len(buffer) ros_msg.fields.append( PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1)) ros_msg.fields.append( PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1)) ros_msg.fields.append( PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1)) ros_msg.fields.append( PointField(name="rgb", offset=12, datatype=PointField.FLOAT32, count=1)) ros_msg.is_bigendian = False ros_msg.point_step = 16 ros_msg.row_step = ros_msg.point_step * ros_msg.width * ros_msg.height ros_msg.is_dense = False ros_msg.data = "".join(buffer) # t2 = rospy.get_time() # print("Pointcloud buffer to Pointcloud msg: " + str(t2-t1) + "s") # t1 = rospy.get_time() self.pointcloud_pub.publish(ros_msg) # t2 = rospy.get_time() # print("Publish: " + str(t2-t1) + "s") # tf_msg = TransformStamped() # tf_msg.header = data.header # tf_msg.header.frame_id = "odom" # tf_msg.child_frame_id = "map" # tf_msg.transform.translation = data.pose.position # tf_msg.transform.rotation = data.pose.orientation # print(tf_msg) # self.br.sendTransformMessage(tf_msg) if (TIME_PROFILING): ofile = open(TIME_PROFILING_PATH + "rgbd_to_pointcloud.txt", 'a') end = time.time() ofile.write('rgbd_to_pointcloud time: %s Seconds' % (end - start)) ofile.write('\n')
def run(): rospy.init_node("r2sonic_node") pub = rospy.Publisher("r2sonic_publisher", PointCloud2, queue_size=1) sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.bind((rospy.get_param('/ip_addr'), int(rospy.get_param('/port')))) fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32,1), PointField('angle', 12, PointField.FLOAT32, 1), PointField('range', 16, PointField.FLOAT32, 1), PointField('intensity', 20, PointField.FLOAT32, 1),] while not rospy.is_shutdown(): s1 = time.time() data, addr = sock.recvfrom(rospy.get_param('/message_size')) packetSize = unpack('!I', data[4:8]) # Skip 8 - 12, 12 - 14 sectionSize = unpack('!H', data[14:16]) # Skip 16 - 52 timeS, timeNS = unpack('!II', data[40:48]) pingNumber, pingPeriod, soundSpeed = unpack('!Iff', data[48:60]) # Skip 60 - 64, 64 - 68, 68 - 72, 80 - 84, 84 - 88, 88 - 92, 92 - 96, 96 - 100, 100-104 # Skip 104 - 106, 106 - 108, 108 - 112, 112 - 116, 116 - 120, 120 - 124 # Skip 124 - 128, 128 - 132, 132 - 136, 136 - 140, 140 - 142 nbPoints, sectionNameA, sectionNameB, sectionSize, scalingFactor = unpack('!HssHf', data[126:136]) ranges = [0 for i in range(nbPoints)] for i in range(nbPoints): ranges[i] = unpack('!H', data[136+i*2:136+2+i*2])[0]*scalingFactor*soundSpeed/2 # Skip [136 + 2 * nbPoints, 136 + 2 * 256 = 648] # Skip 648 - 650, 650 - 652 angleFirst, angleLast = unpack('!ff', data[652:660]) # Skip 660 - 664 zOff, yOff, xOff = unpack('!fff', data[664:676]) # Skip 676 - 680, 680 - 684, 684 - 686, 686 - 688 scalingFactorI1 = unpack('!f', data[688:692])[0] intensities = [] for i in range(nbPoints): intensities.append( unpack('!H', data[692+i*2:692+i*2+2])[0]) # Skip until 692 + 2 * 256 = 1204 # Skip 1204 - 1206, 1206 - 1208 depthGateMin, depthGateMax, depthGateSlope = unpack('!fff', data[1208:1220]) ### ROS stuff ## Header h = Header(frame_id='r2sonic') h.stamp = rospy.Time.now() ## Data data = [] angles = list(np.linspace(angleFirst, angleLast, nbPoints)) for i in range(nbPoints): x = ranges[i]*cos(angles[i]) + xOff y = ranges[i]*sin(angles[i]) + yOff data.append([float(x),float(y), float(zOff), float(angles[i]), float(ranges[i]), float(intensities[i]*scalingFactorI1)]) pointcloud = point_cloud2.create_cloud(h, fields, data) pointcloud.is_dense = True # pointcloud.is_bigendian = True #pcl::fromROSmsg( pub.publish(pointcloud) s2 = time.time()
def read_pcd(filename, cloud_header=None, get_tf=True): if not os.path.isfile(filename): raise Exception("[read_pcd] File does not exist.") string_array = lambda x: x.split() float_array = lambda x: [float(j) for j in x.split()] int_array = lambda x: [int(j) for j in x.split()] word = lambda x: x.strip() headers = [("VERSION", float), ("FIELDS", string_array), ("SIZE", int_array), ("TYPE", string_array), ("COUNT", int_array), ("WIDTH", int), ("HEIGHT", int), ("VIEWPOINT", float_array), ("POINTS", int), ("DATA", word)] header = {} with open(filename, "r") as pcdfile: while len(headers) > 0: line = pcdfile.readline() if line == "": raise Exception( "[read_pcd] EOF reached while looking for headers.") f, v = line.split(" ", 1) if f.startswith("#"): continue if f not in zip(*headers)[0]: raise Exception( "[read_pcd] Field '{}' not known or duplicate.".format(f)) func = headers[zip(*headers)[0].index(f)][1] header[f] = func(v) headers.remove((f, func)) data = pcdfile.read() # Check the number of points if header["VERSION"] != 0.7: raise Exception("[read_pcd] only PCD version 0.7 is understood.") if header["DATA"] != "binary": raise Exception("[read_pcd] Only binary .pcd files are readable.") if header["WIDTH"] * header["HEIGHT"] != header["POINTS"]: raise Exception("[read_pcd] POINTS count does not equal WIDTH*HEIGHT") cloud = PointCloud2() cloud.point_step = sum( [size * count for size, count in zip(header["SIZE"], header["COUNT"])]) cloud.height = header["HEIGHT"] cloud.width = header["WIDTH"] cloud.row_step = cloud.width * cloud.point_step cloud.is_bigendian = False if cloud.row_step * cloud.height > len(data): raise Exception("[read_pcd] Data size mismatch.") offset = 0 for field, size, type, count in zip(header["FIELDS"], header["SIZE"], header["TYPE"], header["COUNT"]): if field != "_": pf = PointField() pf.count = count pf.offset = offset pf.name = field pf.datatype = size_type_to_datatype(size, type) cloud.fields.append(pf) offset += size * count cloud.data = data[0:cloud.row_step * cloud.height] if cloud_header is not None: # cloud.header = header cloud.header = cloud_header # print('This is it, header is ' + str(cloud_header)) else: cloud.header.frame_id = "/pcd_cloud" if get_tf: tf = Transform() tf.translation.x, tf.translation.y, tf.translation.z = header[ "VIEWPOINT"][0:3] tf.rotation.w, tf.rotation.x, tf.rotation.y, tf.rotation.z = header[ "VIEWPOINT"][3:] return cloud, tf return cloud
def sync2bag(): parser = argparse.ArgumentParser(description='Convert the synced lidar point cloud to rosbag') parser.add_argument('outdir', help='output dir for outputbag') parser.add_argument("dir", nargs = "?", default = os.getcwd(), help = "base directory of the dataset, if no directory passed the deafult is current working directory") parser.add_argument("-f", "--first_id", default=0, help = "first id of the sequence (between 0 - last).") parser.add_argument("-l", "--last_id", default=1, help = "last id of the sequence (between 0 - last).") args = parser.parse_args() first_id = int(args.first_id) last_id = int(args.last_id) lidar_front_topic = "/lidar2/points_raw" lidar_left_topic = "/lidar1/points_raw" lidar_right_topic = "/lidar3/points_raw" lidar_back_topic = "/lidar4/points_raw" gnss_pose_topic = "/gnss_pose" lidar_concate_topic = "/concate_points_raw" compression = rosbag.Compression.NONE bag_filename = args.outdir + "/" + "multi_lidar_calib.bag" lidar_front_bin_path = args.dir + "/" + "lidar_front_bin" lidar_left_bin_path = args.dir + "/" + "lidar_left_bin" lidar_right_bin_path = args.dir + "/" + "lidar_right_bin" lidar_back_bin_path = args.dir + "/" + "lidar_back_bin" gnss_pose_path = args.dir + "/" + "vehicle_pose" lidar_1_to_2_gt_6dof_path = args.dir lidar_3_to_2_gt_6dof_path = args.dir lidar_4_to_2_gt_6dof_path = args.dir ##################################################################################################################### # load the extrinsic parameters and form the corresponding transformation matrix lidar_1_to_2_gt_6dof_filename = lidar_1_to_2_gt_6dof_path + "/" + "lidar_1_to_2_gt_6dof.txt" lidar_3_to_2_gt_6dof_filename = lidar_3_to_2_gt_6dof_path + "/" + "lidar_3_to_2_gt_6dof.txt" lidar_4_to_2_gt_6dof_filename = lidar_4_to_2_gt_6dof_path + "/" + "lidar_4_to_2_gt_6dof.txt" with open(lidar_1_to_2_gt_6dof_filename) as f: lidar_1_to_2_gt_6dof = yaml.load(f) with open(lidar_3_to_2_gt_6dof_filename) as f: lidar_3_to_2_gt_6dof = yaml.load(f) with open(lidar_4_to_2_gt_6dof_filename) as f: lidar_4_to_2_gt_6dof = yaml.load(f) lidar_1_to_2_T = translation_matrix((float(lidar_1_to_2_gt_6dof['x']), float(lidar_1_to_2_gt_6dof['y']), float(lidar_1_to_2_gt_6dof['z']))) lidar_1_to_2_R = euler_matrix(float(lidar_1_to_2_gt_6dof['roll'])*math.pi/180, float(lidar_1_to_2_gt_6dof['pitch'])*math.pi/180, float(lidar_1_to_2_gt_6dof['yaw'])*math.pi/180, 'sxyz') lidar_1_to_2_M = concatenate_matrices(lidar_1_to_2_T, lidar_1_to_2_R) lidar_3_to_2_T = translation_matrix((float(lidar_3_to_2_gt_6dof['x']), float(lidar_3_to_2_gt_6dof['y']), float(lidar_3_to_2_gt_6dof['z']))) lidar_3_to_2_R = euler_matrix(float(lidar_3_to_2_gt_6dof['roll'])*math.pi/180, float(lidar_3_to_2_gt_6dof['pitch'])*math.pi/180, float(lidar_3_to_2_gt_6dof['yaw'])*math.pi/180, 'sxyz') lidar_3_to_2_M = concatenate_matrices(lidar_3_to_2_T, lidar_3_to_2_R) lidar_4_to_2_T = translation_matrix((float(lidar_4_to_2_gt_6dof['x']), float(lidar_4_to_2_gt_6dof['y']), float(lidar_4_to_2_gt_6dof['z']))) lidar_4_to_2_R = euler_matrix(float(lidar_4_to_2_gt_6dof['roll'])*math.pi/180, float(lidar_4_to_2_gt_6dof['pitch'])*math.pi/180, float(lidar_4_to_2_gt_6dof['yaw'])*math.pi/180, 'sxyz') lidar_4_to_2_M = concatenate_matrices(lidar_4_to_2_T, lidar_4_to_2_R) ##################################################################################################################### # print(lidar_1_to_2_M) lidar_front_num = len(os.listdir(lidar_front_bin_path)) if last_id > lidar_front_num: print("last id must be smaller then " + "{:0>2d}".format(lidar_front_num)) exit() bag = rosbag.Bag(bag_filename, 'w', compression=compression) frame_id = 0 frame_rate = 10.0 # lidar_fields = [ PointField('x', 0, PointField.FLOAT32, 1), # PointField('y', 4, PointField.FLOAT32, 1), # PointField('z', 8, PointField.FLOAT32, 1), # PointField('intensity', 12, PointField.UINT16, 1), # ] lidar_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), ] # # just for test # lidar_left_bin_filename = lidar_left_bin_path + "/" + "{:0>6d}".format(1) + ".bin" # lidar_left_points = np.fromfile(lidar_left_bin_filename, dtype = np.float32).reshape(-1, 4) # lidar_left_points_hom = lidar_left_points.copy() # lidar_left_points_hom[..., 3] = 1 # # print(lidar_left_points) # lidar_1_to_2_points_hom = np.dot(lidar_1_to_2_M, lidar_left_points_hom.T) # lidar_1_to_2_points = lidar_1_to_2_points_hom.T # lidar_1_to_2_points[..., 3] = lidar_left_points[..., 3] # print(lidar_1_to_2_points) # points = np.concatenate((lidar_1_to_2_points, lidar_left_points),axis=0) # print(points) for index in range(first_id, last_id): print("current index: " + "{:0>6d}".format(index) + "[{:0>6d}-{:0>6d}]".format(first_id, last_id)) frame_id = frame_id + 1 lidar_front_bin_filename = lidar_front_bin_path + "/" + "{:0>6d}".format(index) + ".bin" lidar_left_bin_filename = lidar_left_bin_path + "/" + "{:0>6d}".format(index) + ".bin" lidar_right_bin_filename = lidar_right_bin_path + "/" + "{:0>6d}".format(index) + ".bin" lidar_back_bin_filename = lidar_back_bin_path + "/" + "{:0>6d}".format(index) + ".bin" gnss_pose_filename = gnss_pose_path + "/" + "{:0>6d}".format(index) + ".txt" stamp = rospy.rostime.Time.from_seconds(frame_id/frame_rate) with open(gnss_pose_filename) as f: gnss_pose_6dof = yaml.load(f) gnss_pose_header = Header() gnss_pose_header.frame_id = "gnss" gnss_pose_header.stamp = stamp gnss_poseStamp = PoseStamped() gnss_poseStamp.header = gnss_pose_header gnss_poseStamp.pose.position.x = gnss_pose_6dof['x'] gnss_poseStamp.pose.position.y = gnss_pose_6dof['y'] gnss_poseStamp.pose.position.z = gnss_pose_6dof['z'] quat = quaternion_from_euler(float(gnss_pose_6dof['roll'])*math.pi/180, float(gnss_pose_6dof['pitch'])*math.pi/180, float(gnss_pose_6dof['yaw'])*math.pi/180, axes='sxyz') gnss_poseStamp.pose.orientation.x = quat[0] gnss_poseStamp.pose.orientation.y = quat[1] gnss_poseStamp.pose.orientation.z = quat[2] gnss_poseStamp.pose.orientation.w = quat[3] bag.write(gnss_pose_topic, gnss_poseStamp, stamp) # read the lidar file bin lidar_front_points = np.fromfile(lidar_front_bin_filename, dtype = np.float32).reshape(-1, 4) # print(lidar_front_points) lidar_front_header = Header() lidar_front_header.frame_id = "ls_front" lidar_front_header.stamp = stamp lidar_front_pc2 = point_cloud2.create_cloud(lidar_front_header, lidar_fields, lidar_front_points) bag.write(lidar_front_topic, lidar_front_pc2, stamp) lidar_left_points = np.fromfile(lidar_left_bin_filename, dtype = np.float32).reshape(-1, 4) lidar_left_header = Header() lidar_left_header.frame_id = "ls_left" lidar_left_header.stamp = stamp lidar_left_pc2 = point_cloud2.create_cloud(lidar_left_header, lidar_fields, lidar_left_points) bag.write(lidar_left_topic, lidar_left_pc2, stamp) lidar_right_points = np.fromfile(lidar_right_bin_filename, dtype = np.float32).reshape(-1, 4) lidar_right_header = Header() lidar_right_header.frame_id = "ls_right" lidar_right_header.stamp = stamp lidar_right_pc2 = point_cloud2.create_cloud(lidar_right_header, lidar_fields, lidar_right_points) bag.write(lidar_right_topic, lidar_right_pc2, stamp) lidar_back_points = np.fromfile(lidar_back_bin_filename, dtype = np.float32).reshape(-1, 4) lidar_back_header = Header() lidar_back_header.frame_id = "ls_back" lidar_back_header.stamp = stamp lidar_back_pc2 = point_cloud2.create_cloud(lidar_back_header, lidar_fields, lidar_back_points) bag.write(lidar_back_topic, lidar_back_pc2, stamp) # concate the point cloud from front, left, right and back # transform left points to front coordinates lidar_left_points_hom = lidar_left_points.copy() lidar_left_points_hom[..., 3] = 1 lidar_1_to_2_points_hom = np.dot(lidar_1_to_2_M, lidar_left_points_hom.T) lidar_1_to_2_points = lidar_1_to_2_points_hom.T lidar_1_to_2_points[..., 3] = lidar_left_points[..., 3] # transform right points to front coordinates lidar_right_points_hom = lidar_right_points.copy() lidar_right_points_hom[..., 3] = 1 lidar_3_to_2_points_hom = np.dot(lidar_3_to_2_M, lidar_right_points_hom.T) lidar_3_to_2_points = lidar_3_to_2_points_hom.T lidar_3_to_2_points[..., 3] = lidar_right_points[..., 3] # transform back points to front coordinates lidar_back_points_hom = lidar_back_points.copy() lidar_back_points_hom[..., 3] = 1 lidar_4_to_2_points_hom = np.dot(lidar_4_to_2_M, lidar_back_points_hom.T) lidar_4_to_2_points = lidar_4_to_2_points_hom.T lidar_4_to_2_points[..., 3] = lidar_back_points[..., 3] lidar_concate_points = np.concatenate((lidar_front_points, lidar_1_to_2_points),axis=0) lidar_concate_points = np.concatenate((lidar_concate_points, lidar_3_to_2_points),axis=0) lidar_concate_points = np.concatenate((lidar_concate_points, lidar_4_to_2_points),axis=0) lidar_concate_header = Header() lidar_concate_header.frame_id = "ls_concate" lidar_concate_header.stamp = stamp lidar_concate_pc2 = point_cloud2.create_cloud(lidar_concate_header, lidar_fields, lidar_concate_points) bag.write(lidar_concate_topic, lidar_concate_pc2, stamp) bag.close()
class VisualizeNode(object): """ A ros node to publish training set 2D spherical surface image and point clouds """ HEADER = Header(stamp=rospy.Time(), frame_id='velodyne') 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='intensity', offset=12, datatype=PointField.FLOAT32, count=1), PointField(name='range', offset=16, datatype=PointField.FLOAT32, count=1), PointField(name='label', offset=20, datatype=PointField.FLOAT32, count=1) ] def __init__(self, FLAGS): # ROS self.publisherPointCloud = rospy.Publisher(FLAGS.topic_pcl, PointCloud2, queue_size=1) self.publisherIntensityMap = rospy.Publisher(FLAGS.topic_intensity, ImageMsg, queue_size=1) self.publisherRangeMap = rospy.Publisher(FLAGS.topic_range, ImageMsg, queue_size=1) self.publisherLabelMap = rospy.Publisher(FLAGS.topic_label, ImageMsg, queue_size=1) # Initialize ROS node rospy.init_node('npy_node', anonymous=True) rospy.loginfo("npy_node started.") rospy.loginfo( "Publishing pointclouds from %s in '%s'+'%s'+'%s' topics at %d(hz)...", FLAGS.input_path, FLAGS.topic_intensity, FLAGS.topic_label, FLAGS.topic_pcl, FLAGS.rate) rate = rospy.Rate(FLAGS.rate) counter = 0 npyFiles = [] # Tensorflow os.environ['CUDA_VISIBLE_DEVICES'] = FLAGS.gpu with tf.Graph().as_default(): assert FLAGS.net == 'squeezeSeg' or FLAGS.net == 'squeezeSeg32' or FLAGS.net == 'squeezeSeg16', \ 'Selected neural net architecture not supported: {}'.format(FLAGS.net) if FLAGS.net == 'squeezeSeg': if FLAGS.classes == 'ext': mc = kitti_squeezeSeg_config_ext() # Added ground class else: mc = kitti_squeezeSeg_config() # Original training set mc.LOAD_PRETRAINED_MODEL = False mc.BATCH_SIZE = 1 model = SqueezeSeg(mc) elif FLAGS.net == 'squeezeSeg32': if FLAGS.classes == 'ext': mc = kitti_squeezeSeg32_config_ext() # Added ground class else: mc = kitti_squeezeSeg32_config() # Original training set mc.LOAD_PRETRAINED_MODEL = False mc.BATCH_SIZE = 1 if FLAGS.crf == 1: model = SqueezeSeg32(mc) else: model = SqueezeSeg32x(mc) elif FLAGS.net == 'squeezeSeg16': if FLAGS.classes == 'ext': mc = kitti_squeezeSeg16_config_ext() # Added ground class else: mc = kitti_squeezeSeg16_config() # Original training set mc.LOAD_PRETRAINED_MODEL = False mc.BATCH_SIZE = 1 if FLAGS.crf == 1: # Using conditional random fields (CRF) model = SqueezeSeg16(mc) else: # Disable CRF model = SqueezeSeg16x(mc) saver = tf.train.Saver(model.model_params) with tf.Session(config=tf.ConfigProto( allow_soft_placement=True)) as sess: saver.restore(sess, FLAGS.checkpoint) for file in glob.iglob(FLAGS.input_path): npyFiles.append(file) npyFiles.sort() for file in npyFiles: lidarPrediction = np.load(file).astype(np.float32, copy=False) lidar = lidarPrediction[:, :, :5] lidar_mask = np.reshape( (lidar[:, :, 4] > 0), [mc.ZENITH_LEVEL, mc.AZIMUTH_LEVEL, 1]) lidar = (lidar - mc.INPUT_MEAN) / mc.INPUT_STD pred_cls = sess.run(model.pred_cls, feed_dict={ model.lidar_input: [lidar], model.keep_prob: 1.0, model.lidar_mask: [lidar_mask] }) lidarPrediction[:, :, 5] = pred_cls[0] if rospy.is_shutdown(): break self.npyPublish(lidarPrediction, file) counter = +1 rate.sleep() rospy.logwarn("%d frames published.", counter) def npyPublish(self, npyFile, file): # Point cloud shape (X, 512, 6), where X = 64, 32, 16 # npyFile = np.load(file).astype(np.float32, copy=False) lidar = npyFile[:, :, :5] # x, y, z, i, r label = npyFile[:, :, 5] # label label3D = np.zeros((label.shape[0], label.shape[1], 3)) label3D[np.where(label == 0)] = [1., 0., 0.] label3D[np.where(label == 1)] = [1., 1., 0.] label3D[np.where(label == 2)] = [0., 1., 0.] label3D[np.where(label == 3)] = [0., 1., 1.] label3D[np.where(label == 4)] = [0., 0., 1.] # point cloud for SqueezeSeg segments x = lidar[:, :, 0].reshape(-1) y = lidar[:, :, 1].reshape(-1) z = lidar[:, :, 2].reshape(-1) i = lidar[:, :, 3].reshape(-1) r = lidar[:, :, 4].reshape(-1) #TODO: fix hard-coded label color label = label.reshape(-1) rgb = np.zeros((label.shape[0], 3)) label[np.where(label == 0)] = 0 label[np.where(label == 1)] = 1 #0.1 label[np.where(label == 2)] = 2 #0.2 label[np.where(label == 3)] = 3 #0.3 label[np.where(label == 4)] = 4 #0.4 # Point cloud shape (6, 32768) pc = np.stack((x, y, z, i, r, label)) # Range Map rangeMap = Image.fromarray( (255 * norm(lidar[:, :, 4])).astype(np.uint8)) # Intensity map intensityMap = Image.fromarray( (255 * norm(lidar[:, :, 3])).astype(np.uint8)) # Label map labelMap = Image.fromarray((255 * norm(label3D)).astype(np.uint8)) # Messages to publish msgIntensity = ImageConverter.toROS(intensityMap) msgIntensity.header = VisualizeNode.HEADER msgRange = ImageConverter.toROS(rangeMap) msgRange.header = VisualizeNode.HEADER msgLabel = ImageConverter.toROS(labelMap) msgLabel.header = VisualizeNode.HEADER msgSegment = pc2.create_cloud(header=VisualizeNode.HEADER, fields=VisualizeNode.FIELDS, points=pc.T) self.publisherIntensityMap.publish(msgIntensity) self.publisherRangeMap.publish(msgRange) self.publisherLabelMap.publish(msgLabel) self.publisherPointCloud.publish(msgSegment) filename = file.strip('.npy').split('/')[-1] rospy.loginfo("%s published.", filename)
# spin() simply keeps python from exiting until this node is stopped rospy.spin() direc = 0 def pc2obs_init(): # Configure depth and color streams global bridge, pub bridge = CvBridge() realsense_listener = threading.Thread(target=listener) realsense_listener.start() pub = rospy.Publisher("obs_center", PointCloud2, queue_size=1) 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" def pc2obs(voxel_size = 0.3, plot=False, ros=True): global points_raw, color_image_raw, robot_state, bridge, currentStatus, handle_easy, pub, sim_time #print(points_raw) # if type(points_raw) == type(0) or type(color_image_raw) == type(0): if type(points_raw) == type(0) or sim_time == 0.0: print("NOT CONNECTED") sleep(0.1) return False, False, False t1 = time.time()
def process(): # pub = rospy.Publisher('velodyne_point_data', String, queue_size=10) # while not rospy.is_shutdown(): rospy.init_node('test_velodyne', anonymous=True) # bag_name should be the same for both rosbag and gicp_simplified_result bag_name = "kitti_2011_09_26_drive_0005_synced" bag_dir = "/home/cuberick/raw_data/rosbag/%s.bag" % (bag_name) gicp_output_dir = "/home/cuberick/raw_data/gicp_simplified_output/%s.txt" % ( bag_name) bag = rosbag.Bag(bag_dir) interval = 1 density = 50 duration = rospy.Duration(0.1, 0) number_of_frame = 152 # lengh_of_oxts = number_of_frame # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # Read IMU-to-Velodyne Transformation Matrix tcount = 1 print("===============================================") print("| ---PROGRAM START--- |") print("| |") print("| |") print("| |") print("| Ver.gicp_incremental |") print("===============================================") print print sys.stdout.write("KITTI sequence: %s" % bag_name) print sys.stdout.write("Frame interval: %d, Point density: %d" % (interval, density)) # sys.stdout.flush() print print print("Bag LOADED") print("Please launch rviz") print print sys.stdout.write("\r>>>Read tf info") sys.stdout.flush() # print for topic, msg, t in bag.read_messages("/tf_static"): # if tcount < 1: # break # tcount -= 1 # print count All_tfs = msg.transforms # Extract T_imu_to_velo for transf in All_tfs: if transf.child_frame_id == "velo_link": T_imu_to_velo = transf.transform # Transform between quaternion and Euler T_imu_to_velo_Quaternion_rotation = T_imu_to_velo.rotation T_imu_to_velo_translation = T_imu_to_velo.translation # print(T_imu_to_velo_Quaternion_rotation) quaternion = (T_imu_to_velo_Quaternion_rotation.x, T_imu_to_velo_Quaternion_rotation.y, T_imu_to_velo_Quaternion_rotation.z, T_imu_to_velo_Quaternion_rotation.w) T_imu_to_velo_Euler_rotaiton = tf.transformations.euler_from_quaternion( quaternion) # print(T_imu_to_velo_Euler_rotaiton) roll = T_imu_to_velo_Euler_rotaiton[0] pitch = T_imu_to_velo_Euler_rotaiton[1] yaw = T_imu_to_velo_Euler_rotaiton[2] T_imu_to_velo_homo = np.empty([4, 4], dtype=float) T_imu_to_velo_homo = [ [ math.cos(yaw) * math.cos(pitch), -math.cos(yaw) * math.sin(pitch) * math.sin(roll) + math.sin(yaw) * math.cos(roll), -math.cos(yaw) * math.sin(pitch) * math.cos(roll) - math.sin(yaw) * math.sin(roll), -T_imu_to_velo_translation.x ], [ -math.sin(yaw) * math.cos(pitch), math.sin(yaw) * math.sin(pitch) * math.sin(roll) + math.cos(yaw) * math.cos(roll), -math.sin(yaw) * math.sin(pitch) * math.cos(roll) + math.cos(yaw) * math.sin(roll), -T_imu_to_velo_translation.y ], [ math.sin(pitch), -math.cos(pitch) * math.sin(roll), math.cos(pitch) * math.cos(roll), -T_imu_to_velo_translation.z ], [0, 0, 0, 1] ] # print T_imu_to_velo_homo sys.stdout.write("\r T_imu_to_velo obtained") sys.stdout.flush() # print (" T_imu_to_velo obtained") # print # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # ---->>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # Obtain gicp results sys.stdout.write("\r>>>Read GICP raw data") sys.stdout.flush() gicp_raw = np.loadtxt(gicp_output_dir) gicp_row_length = np.shape(gicp_raw)[0] pose_icp = [None] * number_of_frame pose_icp[0] = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) pose_icp_incr = [None] * number_of_frame pose_icp_incr[0] = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) i = 0 current_starting_row = 0 current_ending_row = 0 accumulate_tf = pose_icp[0] for i in range(number_of_frame - 1): current_starting_row = i * 4 + 0 current_ending_row = i * 4 + 3 current_pose_row_0 = gicp_raw[current_starting_row, :] current_pose_row_1 = gicp_raw[current_starting_row + 1, :] current_pose_row_2 = gicp_raw[current_starting_row + 2, :] current_pose_row_3 = gicp_raw[current_starting_row + 3, :] current_pose = np.zeros(shape=(4, 4)) current = np.matrix([ current_pose_row_0, current_pose_row_1, current_pose_row_2, current_pose_row_3 ]) accumulate_tf = current.dot(accumulate_tf) pose_icp[i + 1] = accumulate_tf pose_icp_incr[i + 1] = current # print current_starting_row # print i # ---->>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # include Tr matrix pose_icp_T = [None] * number_of_frame pose_icp_incr_T = [None] * number_of_frame for i in range(number_of_frame - 1): transfer_pose = np.empty(( 4, 4, )) # print (T_imu_to_velo_homo) # print # print(pose[i]) transfer_pose = np.dot(T_imu_to_velo_homo, pose_icp[i]) transfer_pose_incr = np.dot(T_imu_to_velo_homo, pose_icp_incr[i]) pose_icp_T[i] = np.empty(( 4, 4, )) pose_icp_T[i] = transfer_pose pose_icp_incr_T[i] = np.empty(( 4, 4, )) pose_icp_incr_T[i] = transfer_pose_incr sys.stdout.write("\r Pose_GICP data obtained") sys.stdout.flush() # rospy.sleep(0.5) # Sleeps for 1 sec # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # Read OXTS data OXTS_GPS_raw = np.empty([1, 3], dtype=float) gcount = 1 sys.stdout.write("\r>>>Read OXTS GPS raw data") sys.stdout.flush() # print # rospy.sleep(0.5) # Sleeps for 1 sec for topic, msg, t in bag.read_messages("/kitti/oxts/gps/fix"): # if gcount < 1: # break # gcount -= 1 current_GPS_data = [msg.latitude, msg.longitude, msg.altitude] OXTS_GPS_raw = np.vstack([OXTS_GPS_raw, current_GPS_data]) OXTS_GPS_raw = np.delete(OXTS_GPS_raw, (0), axis=0) sys.stdout.write("\r OSTX GPS raw data obtained") # print # print(OXTS_GPS_raw) sys.stdout.write("\r>>>Read OXTS IMU data") sys.stdout.flush() # print OXTS_IMU_raw = np.empty([1, 3], dtype=float) icount = 3 for topic, msg, t in bag.read_messages("/kitti/oxts/imu"): # if icount < 1: # break # icount -= 1 # print msg IMU_raw = msg.orientation quaternion_IMU = (IMU_raw.x, IMU_raw.y, IMU_raw.z, IMU_raw.w) IMU_data = tf.transformations.euler_from_quaternion(quaternion_IMU) IMU_roll = IMU_data[0] IMU_pitch = IMU_data[1] IMU_heading = IMU_data[2] OXTS_IMU_raw = np.vstack( [OXTS_IMU_raw, [IMU_roll, IMU_pitch, IMU_heading]]) # print IMU_data OXTS_IMU_raw = np.delete(OXTS_IMU_raw, (0), axis=0) # print OXTS_IMU_raw sys.stdout.write("\r OXTS_IMU data obtained") sys.stdout.flush() # print # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # Calculate pose (loadOxtsliteData and convertOxtsToPose) # compute scale from first lat value oxts_first = OXTS_GPS_raw[0][0] scale = math.cos(oxts_first * math.pi / 180.00) # print scale # OXTS_GPS_raw [1] [2] [3] and OXTS_IMU_raw [1] [2] [3] oxts = np.concatenate((OXTS_GPS_raw, OXTS_IMU_raw), axis=1) # print oxts lengh_of_oxts = np.shape(oxts)[0] # print lengh_of_oxts pose_gps = [None] * lengh_of_oxts Tr_0_inv = np.zeros(shape=(4, 4)) isempty = np.zeros(shape=(4, 4)) # a = oxts[0] # print(a) i = 0 for i in range(lengh_of_oxts - 1): if oxts[i] == []: pose_gps[i] = np.empty(( 3, 3, )) * np.nan continue t = np.empty(( 3, 1, )) current_oxts_1 = oxts[i][0] current_oxts_2 = oxts[i][1] er = 6378137 current_t_11 = scale * current_oxts_2 * math.pi * er / 180 current_t_12 = scale * er * math.log( math.tan((90 + current_oxts_1) * math.pi / 360)) current_t_13 = oxts[i][2] t = [[current_t_11], [current_t_12], [current_t_13]] # print t # print # print i # print(oxts[i]) rx = oxts[i][3] ry = oxts[i][4] rz = oxts[i][5] # print (rx) # print (ry) # print (rz) Rx = np.matrix([[1, 0, 0], [0, math.cos(rx), -math.sin(rx)], [0, math.sin(rx), math.cos(rx)]]) Ry = np.matrix([[math.cos(ry), 0, math.sin(ry)], [0, 1, 0], [-math.sin(ry), 0, math.cos(ry)]]) Rz = np.matrix([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]]) R = np.empty(( 3, 3, )) R = np.dot(np.dot(Rz, Ry), Rx) # print (Rx) # print (Ry) # print (Rz) # print R # print current_matrix = np.zeros(shape=(4, 4)) first_three_row = np.concatenate((R, t), axis=1) current_matrix = np.vstack([first_three_row, [0, 0, 0, 1]]) # print first_three_row if np.array_equal(Tr_0_inv, isempty): # print "enter if statement" # print i Tr_0_inv = inv(current_matrix) # if i == 0: # print Tr_0_inv # print four_rows current_pose = np.empty(( 4, 4, )) current_pose = Tr_0_inv.dot(current_matrix) pose_gps[i] = current_pose # print i # print oxts[i] # print pose[i] # raw_input("press ehnter to continue") # ---->>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # include Tr matrix pose_gps_T = [None] * lengh_of_oxts for i in range(lengh_of_oxts - 1): transfer_pose = np.empty(( 4, 4, )) # print (T_imu_to_velo_homo) # print # print(pose[i]) transfer_pose = np.dot(T_imu_to_velo_homo, pose_gps[i]) pose_gps_T[i] = np.empty(( 4, 4, )) pose_gps_T[i] = transfer_pose sys.stdout.write("\r Pose_GPS data obtained") sys.stdout.flush() frame = 0 frame_count = 0 frame_counts = 0 total_frames = 0 frames_left = 0 skipped_count = 0 rejected_count = 0 # for frame in range(0,interval,len(pose_T)): # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # Data summary: # pose_icp_T - gicp pose data with Tr transform # pose_icp_incr_T - incremental pose data # pose_gps_T - gps pose data with Tr transform # pose_T the original variable used, now it is modified # pose_T should be the ekf pose result # Compare data size if len(pose_icp_T) < len(pose_gps_T): frameNo = len(pose_icp_T) else: frameNo = len(pose_gps_T) # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # Read velodyne info sys.stdout.write("\r>>>Read Velodyne point data") sys.stdout.flush() print print # all_points = np.empty([1,3],dtype=float) current_point_set = np.empty(( 999999, 3, )) * np.NaN vcount = 5 bag_count = -1 total_msg_no = 0 for topic, msg, t in bag.read_messages("/kitti/velo/pointcloud"): # bag_count += 1 # if (bag_count) % interval != 0: # continue total_msg_no += 1 all_points = [np.empty([1, 4], dtype=float)] * total_msg_no for topic, msg, t in bag.read_messages("/kitti/velo/pointcloud"): # transformed_points = np.empty((1,3,)) transformed_points = np.empty(( 999999, 3, )) * np.NaN bag_count += 1 # if (bag_count) % interval != 0: # continue # if vcount < 1: # break # vcount -= 1 # print("counting cycles") # print vcount frame_count += 1 total_frames = len(pose_icp_T) / interval total_frames = math.ceil(total_frames) frames_left = total_frames - frame_count + 1 info_of_frame = "Processing scan No.%d , %d remaining" % (frame_count, frames_left) sys.stdout.write("\r%s" % info_of_frame) sys.stdout.flush() # sys.stdout.write(" ~~~~~~working hard >.< please wait!~~~~~~~") # print # print vcount data_length = len(msg.data) ## msg is of type PointCloud2 raw_data = pc2.read_points(msg) point_count_raw = 0 for point in raw_data: current_point = [point[0], point[1], point[2]] # if point[0] > 4: try: # print point_count_raw # print current_point current_point_set[point_count_raw] = current_point point_count_raw += 1 except: # print ".^. skip recording this point" skipped_count += 1 continue current_point_set = np.delete(current_point_set, (0), axis=0) current_point_set = current_point_set[~np.isnan(current_point_set).any( axis=1)] velo = current_point_set if np.shape(velo)[0] < 2: continue j = 0 point_count = -1 for j in range(np.shape(velo)[0]): try: point_count += 1 if (point_count + 1) % density != 0: continue # print;print pose_ekf pose_a = pose_icp_T[bag_count] point = velo[j] point_a = point[np.newaxis, :].T # print point_a point_b = np.vstack([point_a, [1]]) point_c = np.dot(pose_a, point_b) point_c = point_c[np.newaxis, :].T point_c = np.delete(point_c, [3], axis=1) # print; print point_c transformed_points[j] = point_c except: # print;print "except" continue transformed_points = transformed_points[~np.isnan(transformed_points). any(axis=1)] # print; print transformed_points try: transformed_points = np.delete(transformed_points, (0), axis=0) except: continue all_points[frame_count - 1] = transformed_points all_points[frame_count - 1] = np.delete(all_points[frame_count - 1], (0), axis=0) # all_points = np.vstack([all_points, transformed_points]) # all_points = np.delete(all_points, (0), axis=0) # print(all_points) # a = all_points.shape # print(a) # print frame_count sys.stdout.write("\rVelodyne point data processing finished") sys.stdout.flush() # bag.close() # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # ---->>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> # pose = # subprocess.call(['spd-say','start publishing']) # print all_points # print # print(">>>>>>>>>>>>>>>>>>>>>>>>>>>>><<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<") # prsint(">>>>>>>>>>>>>>>>>>>>>>>>>>>>><<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<") sys.stdout.write("\rProcessing completed, generating system report") # print # sys.stdout.flush() # print # a = type(all_points) b = np.shape(all_points) # print;print a sys.stdout.write(" Total frames:") print b[0] sys.stdout.write(" Skipped points:") print skipped_count sys.stdout.write(" Rejected points:") print rejected_count print # print # print ("Start visualising...") pcl_pub = rospy.Publisher("/gicp_visu", PointCloud2, queue_size=10) rospy.loginfo("Publisher started at: /gicp_visu") rospy.sleep(1.) rospy.loginfo("Publishing...") print print print bag.close() current_visual_set = np.empty([1, 3]) while (1): raw_input("\r... waiting for instruction") # sys.stdout.write("Start visualising...") sys.stdout.flush() current_visual_set = np.empty([1, 3]) k = 0 for k in range(total_msg_no): sys.stdout.write("\rVisualising frame %d" % k) sys.stdout.flush() header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'map' 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) ] try: current_visual_set = np.concatenate( (current_visual_set, all_points[k])) # a = type(current_visual_set) # print;print a except: continue current_visual_set_list = current_visual_set.tolist() # print all_points processed_data = pc2.create_cloud_xyz32(header, current_visual_set_list) rospy.sleep(duration) # [[1, 1, 1]] # a = [[1, 1, 1]] # b = type(a) # print b pcl_pub.publish(processed_data) sys.stdout.write("\rVisualisation complete") sys.stdout.flush()
#!/usr/bin/env python # import roslib import rospy from std_msgs.msg import Header from sensor_msgs.msg import PointCloud2, PointField import numpy as np import sensor_msgs.point_cloud2 as pc2 import py3d pub = rospy.Publisher('/output', PointCloud2, queue_size=1) tmp_pcd_name = "/tmp/tmp_cloud.pcd" 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='rgb', offset=12, datatype=PointField.UINT32, count=1), ] TEST_POINTS = [ [0.3, 0.0, 0.0, 0xff0000], [0.0, 0.3, 0.0, 0x00ff00], [0.0, 0.0, 0.3, 0x0000ff], ] def convert_pcl(data): import py3d header = '''# .PCD v0.7 - Point Cloud Data file format VERSION 0.7
def sendToRVIZ(): # ------------------------------------------------------------------------------------------------------ # set variables # ------------------------------------------------------------------------------------------------------ point_cloud_pub = rospy.Publisher('point_cloud', PointCloud2) location_debug_pub = rospy.Publisher('location_debug', PointCloud) bbox_debug_pub = rospy.Publisher('bbox_debug', PointCloud) bb_ground_truth_pub = rospy.Publisher("bb_ground_truth", BoundingBoxArray) bb_pred_guess_1_pub = rospy.Publisher("bb_pred_guess_1", BoundingBoxArray) bb_pred_guess_2_pub = rospy.Publisher("bb_pred_guess_2", BoundingBoxArray) pub_image = rospy.Publisher('image', Image) rospy.init_node('talker', anonymous=True) #giving some time for the publisher to register rospy.sleep(0.1) counter = 0 prediction_min_score = 0.1 # ------------------------------------------------------------------------------------------------------ # set parameter # ------------------------------------------------------------------------------------------------------ show_annotations = True show_ground_truth = True mode = "testing_sampled" # Options: live, training_unsampled, testing_sampled, testing_unsampled, gt_database_val, gt_database # training_sampled is not here since the sampling is done during training, to see that you have to use the # "save" param in "load_data.py" and "send_to_rviz.py" # ------------------------------------------------------------------------------------------------------ # Load Pointcloud ground truths # ------------------------------------------------------------------------------------------------------ if mode == "testing_unsampled": # load gt with open( "/home/makr/Documents/data/pedestrian_3d_own/1/object/kitti_infos_val.pkl", "rb") as file: kitti_infos = pickle.load(file) # load annos # load point clouds point_cloud_path = "/home/makr/Documents/data/pedestrian_3d_own/1/object/testing/velodyne_unsampled" # TESTING SAMPLED/UNSAMPLED DATA (depending on what you copy into the folder) show_annotations = False if mode == "testing_sampled": # load gt with open( "/home/makr/Documents/data/pedestrian_3d_own/1/object/kitti_infos_val_sampled.pkl", "rb") as file: kitti_infos = pickle.load(file) # load annos # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_377/out_dir_eval_results/result_epoch_22.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_399/out_dir_eval_results/result_epoch_8.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) with open( "/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_411/out_dir_eval_results/result_epoch_16.pkl", "rb") as file: thesis_eval_dict = pickle.load(file) with open( "/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_411/out_dir_eval_results/result.pkl", "rb") as file: thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_496/out_dir_eval_results/result.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_420/out_dir_eval_results/result_epoch_21.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # load point clouds point_cloud_path = "/home/makr/Documents/data/pedestrian_3d_own/1/object/testing/velodyne" # TESTING SAMPLED/UNSAMPLED DATA (depending on what you copy into the folder) if mode == "training_unsampled": # load gt # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_136/out_dir_eval_results/result_epoch_4.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) with open( "/home/makr/Documents/data/pedestrian_3d_own/1/object/kitti_infos_train.pkl", "rb") as file: kitti_infos = pickle.load(file) # load annos # load point clouds point_cloud_path = "/home/makr/Documents/data/pedestrian_3d_own/1/object/training/velodyne" show_annotations = False if mode == "live": # load gt # load annos # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_127/out_dir_eval_results/result_epoch_15_velodyne_2.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_368/out_dir_eval_results/result_e22_ve2.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_377/out_dir_eval_results/result_e33_ve1.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_400/out_dir_eval_results/result_epoch_0.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_411/out_dir_eval_results/result_e16_ve2.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_448/out_dir_eval_results/result_epoch_9.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_465/out_dir_eval_results/result.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) #with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_488/out_dir_eval_results/result_epoch_40.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_496/out_dir_eval_results/result_epoch_80.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # gut sind: 21,40(0.6),42(0.6) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_496/out_dir_eval_results/result_e21_ve5_2.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) with open( "/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_496/out_dir_eval_results/result.pkl", "rb") as file: thesis_eval_dict = pickle.load(file) # with open("/home/makr/Documents/uni/TU/3.Master/experiments/own/tf_3dRGB_pc/out/model_496/out_dir_eval_results/result_e21_ve5_2.pkl", "rb") as file: # thesis_eval_dict = pickle.load(file) # # load point clouds point_cloud_path = "/home/makr/Documents/data/pedestrian_3d_own/1/object/testing_live/velodyne" show_ground_truth = False # to false since we dont have annos for live data if mode == "gt_database_val": # load gt # load annosz # load point clouds point_cloud_path = "/home/makr/Desktop/temp/pedestrian_3d_own/1/object/gt_database_val" if mode == "gt_database": # load gt # load annos # load point clouds point_cloud_path = "/home/makr/Desktop/temp/pedestrian_3d_own/1/object/gt_database" show_annotations = False show_ground_truth = False # ------------------------------------------------------------------------------------------------------ # load pointclouds from folder and order numerically # ------------------------------------------------------------------------------------------------------ import os velodyne_data = [] for root, dirs, files in os.walk(point_cloud_path, topdown=False): files = [x[:-4] for x in files] # cut the .png for name in sorted( files ): # order the list // !!!!! every 10s elements is out of order since name = name + ".pkl" # append .png again file = os.path.join(root, name) with open(file, 'rb') as file: velodyne_data.append(pickle.load(file, encoding="latin1")) # ------------------------------------------------------------------------------------------------------ # iterate over the pointcloud files # ------------------------------------------------------------------------------------------------------ assert (len(velodyne_data) > 0) while not rospy.is_shutdown() and counter < len(velodyne_data): rospy.sleep(0.0) try: point_cloud = velodyne_data[counter] # ------------------------------------------------------------------------------------------------------ # use our custom calibtrations # ------------------------------------------------------------------------------------------------------ calib = { "R0_rect": np.array([1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]).reshape(3, 3), "Tr_velo_to_cam": np.array([ 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0 ]).reshape(3, 4) } # ------------------------------------------------------------------------------------------------------ # Create and Publish Point Cloud # ------------------------------------------------------------------------------------------------------ # declaring pointcloud my_awesome_pointcloud = PointCloud() # filling pointcloud header header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'camera_color_frame' my_awesome_pointcloud.header = header fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), ] # https://gist.github.com/lucasw/ea04dcd65bc944daea07612314d114bb pc2 = point_cloud2.create_cloud(header, fields, point_cloud) point_cloud_pub.publish(pc2) # ------------------------------------------------------------------------------------------------------ # Create and Publish Ground Truth Boxes # ------------------------------------------------------------------------------------------------------ if show_ground_truth: annotation_gt = kitti_infos[counter] centers_gt, dims_gt, angles_gt = kitti_anno_to_corners( calib, annotation_gt["annos"]) centers_gt = centers_gt + [0.0, 0.0, 1.0] send_3d_bbox(centers_gt, dims_gt, angles_gt, bb_ground_truth_pub, header) # ------------------------------------------------------------------------------------------------------ # Create and Publish Annos # ------------------------------------------------------------------------------------------------------ if show_annotations: annotations = thesis_eval_dict[counter] detection_anno = remove_low_score(annotations, prediction_min_score) if len(detection_anno["score"]) > 0: print(detection_anno["score"]) centers, dims, angles = kitti_anno_to_corners( calib, detection_anno ) # [a,b,c] -> [c,a,b] (camera to lidar coords) #centers = centers + [0.0,0.0,1.9] # Epoch 2 #centers = centers + [0.0,0.0,2.3] #Epoch 3 #centers = centers + [0.0,0.0,2.4] # live centers = centers + [ 0.0, 0.0, 0.9 ] # live # TODO: müsste eigentlich um höhe/2 angehoben werden weil Pointpillars die z position am boden angibt der bbox und rviz die z mitte der höhe der bbox send_3d_bbox(centers, dims, angles, bb_pred_guess_1_pub, header) if (counter % 100 == 0): print(counter) counter = counter + 1 except IOError as e: pass
# 利用变换矩阵T,将相机坐标转换成世界坐标 # Code here pointWorld = np.dot(T, point) # 将世界坐标添加到cloudWorld中 # Code here cloudWorld.append( pointWorld[0:3].tolist() + [color[v][u][2], color[v][u][1], color[v][u][0]]) # ROS相关操作,将点云信息发布出去 rospy.init_node('pointcloud', anonymous=True) pub_cloud = rospy.Publisher("/points", PointCloud2) while not rospy.is_shutdown(): pcloud = PointCloud2() # make point cloud fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('r', 12, PointField.FLOAT32, 1), PointField('g', 16, PointField.FLOAT32, 1), PointField('b', 20, PointField.FLOAT32, 1) ] pcloud = point_cloud2.create_cloud(pcloud.header, fields, cloudWorld) # pcloud = point_cloud2.create_cloud_xyz32(pcloud.header, cloudWorld) pcloud.header.frame_id = "/map" pub_cloud.publish(pcloud) rospy.sleep(1.0)
cloud.header.stamp = image_grabber.image_rgb.header.stamp cloud.header.frame_id = image_grabber.image_rgb.header.frame_id cloud.height = image_grabber.image_rgb.height cloud.width = image_grabber.image_rgb.width cloud.point_step = 32 cloud.row_step = 20480 # Jam the numpy XYZ/RGB data into the cloud points_arr = image_grabber.array_xyz.astype('float32') image_rgb = image_grabber.image_rgb.data cloud.data = numpy.rec.fromarrays((points_arr, image_rgb), names=('xyz', 'rgb')) # Generate 'fields' attribute of PointCloud2 according to Kinect conventions cloud.fields.append( PointField(name='x', offset=0, datatype=7, count=1)) cloud.fields.append( PointField(name='y', offset=4, datatype=7, count=1)) cloud.fields.append( PointField(name='z', offset=8, datatype=7, count=1)) cloud.fields.append( PointField(name='rgb', offset=16, datatype=7, count=1)) # Publish! image_grabber.pub_cloud.publish(data=cloud.data, fields=cloud.fields, header=cloud.header, height=cloud.height, width=cloud.width, point_step=cloud.point_step, row_step=cloud.row_step)
if len(cloud) == 0: for v, u in zip(x, y): color = cv_image[v, u] Z = depth_image[0][v, u] if Z == 0: continue X = (u - np.array(K[0])[0, 2]) * Z / np.array(K[0])[0, 0] Y = (v - np.array(K[0])[0, 5]) * Z / np.array(K[0])[0, 0] a = 255 rgb = struct.unpack('I', struct.pack('BBBB', color[2], color[1], color[0], a))[0] cloud.append([X, Y, Z, rgb]) # Wait for point cloud to arrive. while len(cloud) == 0: rospy.sleep(0.01) 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), ] rim_sub = rospy.Subscriber('/rim_points', my_msg, rimCallback) print len(cloud) print 'Publishing point cloud...' pub = rospy.Publisher("my_cloud", PointCloud2, queue_size=2) header = Header() header.frame_id = "/camera_depth_optical_frame" pc2 = point_cloud2.create_cloud(header, fields, cloud) s = raw_input('Hit [ENTER] to publish') pub.publish(pc2)
#!/usr/bin/env python # This program publishes a pointcloud2 which has much shorter data # than what is described by width, height, and point_step. RViz # should catch this and show an error instead of crashing. import rospy from sensor_msgs.msg import PointCloud2 from sensor_msgs.msg import PointField rospy.init_node( 'bad_pointcloud_publisher' ) pfx = PointField() pfx.name = 'x' pfx.offset = 0 pfx.datatype = 7 pfx.count = 1 pfy = PointField() pfy.name = 'y' pfy.offset = 4 pfy.datatype = 7 pfy.count = 1 pfz = PointField() pfz.name = 'z' pfz.offset = 8 pfz.datatype = 7 pfz.count = 1 pc = PointCloud2()
def create_cloud_xyzi32(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)] return pcl2.create_cloud(header, fields, points)
def __init__(self): rospy.init_node('d415_camera_node') rootDir = os.path.dirname(os.path.abspath(__file__)) imgsPath = os.path.abspath(os.path.join(rootDir, "..")) + "/images" self.ns = rospy.get_namespace() self.fps = rospy.get_param('~fps', 60) self.update_rate = rospy.get_param('~update_rate', 90) self.save_path = rospy.get_param('~save_path', imgsPath) self.flag_save_imgs = rospy.get_param('~flag_save_imgs', False) self.use_float_depth = rospy.get_param('~use_float_depth', True) self.publishTf = rospy.get_param('~publish_tf', True) self.enable_pc = rospy.get_param('~enable_pointcloud', False) self.get_aligned = rospy.get_param('~use_aligned', True) self.cam_name = rospy.get_param('~camera_name', "d415") self.base_tf_frame = rospy.get_param('~base_tf_frame', "base_link") cam_base_tf_frame = rospy.get_param('~cam_base_tf_frame', "_base_link") rgb_optical_tf_frame = rospy.get_param('~rgb_optical_tf_frame', "/rgb_optical_frame") depth_optical_tf_frame = rospy.get_param('~depth_optical_tf_frame', "/depth_optical_frame") self.cam_base_tf_frame = self.cam_name + cam_base_tf_frame self.rgb_optical_tf_frame = self.cam_name + rgb_optical_tf_frame self.depth_optical_tf_frame = self.cam_name + depth_optical_tf_frame depth_image_topic = rospy.get_param('~depth_image_topic', "/depth/image_raw") image_topic_rgb = rospy.get_param('~rgb_image_topic', "/rgb/image_raw") depth_info_topic = rospy.get_param('~depth_info_topic', "/depth/camera_info") rgb_info_topic = rospy.get_param('~rgb_info_topic', "/rgb/camera_info") cloud_topic = rospy.get_param('~cloud_topic', "/cloud") self.depth_image_topic = self.ns + self.cam_name + depth_image_topic self.image_topic_rgb = self.ns + self.cam_name + image_topic_rgb self.depth_info_topic = self.ns + self.cam_name + depth_info_topic self.rgb_info_topic = self.ns + self.cam_name + rgb_info_topic self.cloud_topic = self.ns + self.cam_name + cloud_topic self.depth_info_pub = rospy.Publisher(self.depth_info_topic, CameraInfo, queue_size=10) self.rgb_info_pub = rospy.Publisher(self.rgb_info_topic, CameraInfo, queue_size=10) self.rgb_pub = rospy.Publisher(self.image_topic_rgb, Image, queue_size=10) self.depth_pub = rospy.Publisher(self.depth_image_topic, Image, queue_size=10) if (self.enable_pc): self.cloud_pub = rospy.Publisher(self.cloud_topic, PointCloud2, queue_size=10) else: self.cloud_pub = None self.bridge = CvBridge() self.br = tf.TransformBroadcaster() res = (848, 480) self.cam = CameraD415(flag_save=False, use_statistics=False, fps=self.fps, depth_resolution=res, verbose=True) self.intr = self.cam.get_intrinsics() self.extr = self.cam.get_extrinsics() fxd = self.intr["depth"].fx fyd = self.intr["depth"].fy ppxd = self.intr['depth'].ppx ppyd = self.intr['depth'].ppy fxc = self.intr["color"].fx fyc = self.intr["color"].fy ppxc = self.intr['color'].ppx ppyc = self.intr['color'].ppy self.focal = [fxd, fyd] self.ppoint = [ppxd, ppyd] self.baseline = self.extr.translation[0] print("""Camera Info: ------------ Color: fx, fy: %.2f, %.2f cx, cy: %.2f, %.2f Depth: fx, fy: %.2f, %.2f cx, cy: %.2f, %.2f baseline: %.2f ------------""" % (fxc, fyc, ppxc, ppyc, fxd, fyd, ppxd, ppyd, self.baseline)) self.rgb_info_msg = CameraInfo() self.rgb_info_msg.header.frame_id = self.rgb_optical_tf_frame self.rgb_info_msg.width = self.intr['color'].width self.rgb_info_msg.height = self.intr['color'].height self.rgb_info_msg.distortion_model = "plumb_bob" self.rgb_info_msg.K = [fxc, 0, ppxc, 0, fyc, ppyc, 0, 0, 1] self.rgb_info_msg.D = [0, 0, 0, 0] self.rgb_info_msg.P = [fxc, 0, ppxc, 0, 0, fyc, ppyc, 0, 0, 0, 1, 0] self.depth_info_msg = CameraInfo() self.depth_info_msg.header.frame_id = self.depth_optical_tf_frame self.depth_info_msg.width = self.intr['depth'].width self.depth_info_msg.height = self.intr['depth'].height self.depth_info_msg.distortion_model = "plumb_bob" self.depth_info_msg.K = [fxd, 0, ppxd, 0, fyd, ppyd, 0, 0, 1] self.depth_info_msg.D = [0, 0, 0, 0] self.depth_info_msg.P = [fxd, 0, ppxd, 0, 0, fyd, ppyd, 0, 0, 0, 1, 0] self.pc2_msg_fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] self.r = rospy.Rate(self.update_rate) self.rgb = [] self.depth = [] self.disparity = [] self.count = 0 self.camcount = 0 self.prev_func_count = None self.flag_kill_thread = False if self.flag_save_imgs: self.rgbDir = os.path.join(self.save_path, "rgb") self.depthDir = os.path.join(self.save_path, "depth") self.obsDir = os.path.join(self.save_path, "processed") dirs = [self.save_path, self.rgbDir, self.depthDir, self.obsDir] for dir in dirs: if not os.path.exists(dir): os.makedirs(dir) print("Created directory \'%s\' " % dir) else: print("Directory \'%s\' already exists" % dir) print("[INFO] d415_camera_node --- Started...") if (self.enable_pc): self.cloudPubThread = Thread(target=self.cloudPubFunc, args=()) self.cloudPubThread.start() else: self.cloudPubThread = None
def xyz_point_cloud(): msg = PointCloud2() msg.is_dense = True msg.is_bigendian = True # Unordered point cloud msg.height = 1 msg.point_step = 12 # 32bit float x, y, and z x_field = PointField() x_field.name = 'x' x_field.offset = 0 x_field.datatype = PointField.FLOAT32 x_field.count = 1 y_field = PointField() y_field.name = 'y' y_field.offset = 4 y_field.datatype = PointField.FLOAT32 y_field.count = 1 z_field = PointField() z_field.name = 'z' z_field.offset = 8 z_field.datatype = PointField.FLOAT32 z_field.count = 1 msg.fields.extend((x_field, y_field, z_field)) return msg
last_ring += 1 last_deg = deg[i, 0] ring[i, 0] = last_ring if last_ring == 0: stop_time = t break points_array = numpy.append(points_array, ring, axis=1) # velodyne way order azimuth = numpy.arctan2(y, x) azimuth -= azimuth[0] azimuth[azimuth < 0] += 2*numpy.pi points_array = points_array[azimuth.argsort()] points_array = numpy.flip(points_array, axis=0) points_array = numpy.append(points_array, intensity, axis=1) points_array[:,[3,4]] = points_array[:,[4,3]] 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)] new_msg = pcl2.create_cloud(msg.header, fields, points_array) new_bag.write( '/carla/ego_vehicle/lidar/lidar1/velodyne_points0', new_msg, t=t) distance = numpy.linalg.norm(points_array[:,0:3],axis=1) noise = numpy.random.normal(0, 0.03, distance.shape) ratio = (distance+noise)/distance points_array[:, 0] = points_array[:, 0]*ratio points_array[:, 1] = points_array[:, 1]*ratio points_array[:, 2] = points_array[:, 2]*ratio new_msg = pcl2.create_cloud(msg.header, fields, points_array) new_bag.write(
def read_pcd(filename, cloud_header=None, get_tf=True): if not os.path.isfile(filename): raise Exception("[read_pcd] File does not exist.") string_array = lambda x: x.split() float_array = lambda x: [float(j) for j in x.split()] int_array = lambda x: [int(j) for j in x.split()] word = lambda x: x.strip() headers = [("VERSION", float), ("FIELDS", string_array), ("SIZE", int_array), ("TYPE", string_array), ("COUNT", int_array), ("WIDTH", int), ("HEIGHT", int), ("VIEWPOINT", float_array), ("POINTS", int), ("DATA", word)] header = {} with open(filename, "r") as pcdfile: while len(headers) > 0: line = pcdfile.readline() if line == "": raise Exception("[read_pcd] EOF reached while looking for headers.") f, v = line.split(" ", 1) if f.startswith("#"): continue if f not in zip(*headers)[0]: raise Exception("[read_pcd] Field '{}' not known or duplicate.".format(f)) func = headers[zip(*headers)[0].index(f)][1] header[f] = func(v) headers.remove((f, func)) data = pcdfile.read() # Check the number of points if header["VERSION"] != 0.7: raise Exception("[read_pcd] only PCD version 0.7 is understood.") if header["DATA"] != "binary": raise Exception("[read_pcd] Only binary .pcd files are readable.") if header["WIDTH"] * header["HEIGHT"] != header["POINTS"]: raise Exception("[read_pcd] POINTS count does not equal WIDTH*HEIGHT") cloud = PointCloud2() cloud.point_step = sum([size * count for size, count in zip(header["SIZE"], header["COUNT"])]) cloud.height = header["HEIGHT"] cloud.width = header["WIDTH"] cloud.row_step = cloud.width * cloud.point_step cloud.is_bigendian = False if cloud.row_step * cloud.height > len(data): raise Exception("[read_pcd] Data size mismatch.") offset = 0 for field, size, type, count in zip(header["FIELDS"], header["SIZE"], header["TYPE"], header["COUNT"]): if field != "_": pass pf = PointField() pf.count = count pf.offset = offset pf.name = field pf.datatype = size_type_to_datatype(size, type) cloud.fields.append(pf) offset += size * count cloud.data = data[0:cloud.row_step * cloud.height] if cloud_header is not None: cloud.header = header else: cloud.header.frame_id = "/pcd_cloud" if get_tf: tf = Transform() tf.translation.x, tf.translation.y, tf.translation.z = header["VIEWPOINT"][0:3] tf.rotation.w, tf.rotation.x, tf.rotation.y, tf.rotation.z = header["VIEWPOINT"][3:] return cloud, tf return cloud
def save_data2node(bag, kitti, kitti_type, initial_time): CAM2BASE = np.array([[ -1.857739385241e-03, -9.999659513510e-01, -8.039975204516e-03, -4.784029760483e-03 ], [ -6.481465826011e-03, 8.051860151134e-03, -9.999466081774e-01, -7.337429464231e-02 ], [ 9.999773098287e-01, -1.805528627661e-03, -6.496203536139e-03, -3.339968064433e-01 ], [0, 0, 0, 1]]) tf_origin = np.array([[0, 0, 1, 0], [-1, 0, 0, 0], [0, -1, 0, 0], [0, 0, 0, 1]]) if kitti_type.find("odom") != -1: timestamps = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps) assert (len(kitti.T_w_cam0)) == len(kitti.velo) assert (len(kitti.velo)) == len(kitti.label) print("{} vs {} vs {}. Condition satisfied.".format( len(kitti.T_w_cam0), len(kitti.velo), len(kitti.label))) iterable = zip(timestamps, kitti.velo, kitti.label, kitti.T_w_cam0) bar = progressbar.ProgressBar() count = 0 for timestamp, scan, label, tf_matrix_cam in bar(iterable): tmp = np.matmul(tf_matrix_cam, CAM2BASE) tf_matrix = np.matmul(tf_origin, tmp) out = node() tf_msg = TFMessage() tf_stamped = TransformStamped() tf_stamped.header.stamp = rospy.Time.from_sec(timestamp) tf_stamped.header.frame_id = '/map' tf_stamped.child_frame_id = 'camera_left' t = tf_matrix[0:3, 3] q = tf.transformations.quaternion_from_matrix(tf_matrix) transform = Transform() transform.translation.x = t[0] transform.translation.y = t[1] transform.translation.z = t[2] transform.rotation.x = q[0] transform.rotation.y = q[1] transform.rotation.z = q[2] transform.rotation.w = q[3] tf_stamped.transform = transform tf_msg.transforms.append(tf_stamped) bag.write('/tf', tf_msg, tf_msg.transforms[0].header.stamp) pose_msg = Pose() pose_msg.position.x = t[0] pose_msg.position.y = t[1] pose_msg.position.z = t[2] pose_msg.orientation.x = q[0] pose_msg.orientation.y = q[1] pose_msg.orientation.z = q[2] pose_msg.orientation.w = q[3] header = Header() header.seq = kitti.frame_range[count] header.frame_id = "/map" header.stamp = rospy.Time.from_sec(timestamp) 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) ] scan_xyz = scan[:, :3] label_float = label.astype(np.float32) scan_w_label = np.concatenate((scan_xyz, label_float), axis=1) pcl_msg = pcl2.create_cloud(header, fields, scan_w_label) pcl_raw_msg = pcl2.create_cloud(header, fields, scan) out.header = header out.odom = pose_msg out.lidar = pcl_msg bag.write('/node/combined/optimized', out, out.header.stamp) bag.write('/debug/pc_raw', pcl_raw_msg, out.header.stamp) bag.write('/debug/pc_label', pcl_msg, out.header.stamp) count += 1
def main(args): #prepare the listener global socket socket.connect("tcp://" + ip + ":" + port) socket.setsockopt_string(zmq.SUBSCRIBE, topic ) #setup some opencv windows cv2.namedWindow("Listener Left Camera") cv2.namedWindow("Listener Right Camera") #Start the main loop while True: message_in = socket.recv(10*1024) #For some reason I have to make a deep copy of the message. Otherwise, when, the second time I received the message I got a truncated message error. This solves it so I did not worry about it anymore. message = copy.deepcopy(message_in) #Deserialization or unmarshalling #We use message[4:] because we know the first four bytes are #"777 " and we only want to give the data to the parser (not the topic name sd.ParseFromString(message[4:]) #Getting the left image cv_left_image = np.zeros((sd.left_image.height, sd.left_image.width, 3), np.uint8) cv_left_image.data = sd.left_image.data #Getting the right image cv_right_image = np.zeros((sd.right_image.height, sd.right_image.width, 3), np.uint8) cv_right_image.data = sd.right_image.data #Getting the point cloud point_cloud_msg = PointCloud2() point_cloud_msg.height = sd.point_cloud.height point_cloud_msg.width = sd.point_cloud.width for field in sd.point_cloud.fields: point_field = PointField() point_field.name = field.name point_field.offset = field.offset point_field.datatype = field.datatype point_field.count = field.count point_cloud_msg.fields.append(point_field) point_cloud_msg.is_bigendian = sd.point_cloud.is_bigendian point_cloud_msg.is_bigendian = sd.point_cloud.is_bigendian point_cloud_msg.point_step = sd.point_cloud.point_step point_cloud_msg.row_step = sd.point_cloud.row_step point_cloud_msg.data = sd.point_cloud.data #Getting the NavSatFix nav_sat_fix = NavSatFix() nav_sat_fix.latitude = sd.nav_sat_fix.latitude nav_sat_fix.longitude = sd.nav_sat_fix.longitude nav_sat_fix.altitude = sd.nav_sat_fix.altitude nav_sat_fix.status.status = sd.nav_sat_fix.status.status nav_sat_fix.status.service = sd.nav_sat_fix.status.service #Getting the Odometry odometry = Odometry() #and then we can copy every field ... no need in this example #--------------------------------- #Visualizing and /or printing the received data #--------------------------------- print("Received new message with stamp:\n" + str(sd.header.stamp)) print("First 10 points x,y,z and rgb (packed in float32) values (just for debug)") count = 0 for p in pc2.read_points(point_cloud_msg, field_names = ("x", "y", "z", "rgb"), skip_nans=True): print " x : %f y: %f z: %f rgb: %f" %(p[0],p[1],p[2],p[3]) count = count + 1 if count > 10: break print("GPS data:") print("Latitude =" + str(nav_sat_fix.latitude)) print("Longitude =" + str(nav_sat_fix.longitude)) print("Altitude =" + str(nav_sat_fix.altitude)) print("Status =" + str(nav_sat_fix.status.status)) print("Service =" + str(nav_sat_fix.status.service)) print("Odometry data (only position for example):") print("odom.pose.pose.position.x =" + str(sd.odometry.pose.pose.position.x)) print("odom.pose.pose.position.y =" + str(sd.odometry.pose.pose.position.y)) print("odom.pose.pose.position.z =" + str(sd.odometry.pose.pose.position.z)) cv2.imshow("Listener Left Camera", cv_left_image) cv2.imshow("Listener Right Camera", cv_right_image) cv2.waitKey(30)
def save_velo_data(bag, kitti, velo_frame_id, topic, bridge): print("Exporting velodyne data") #lut to map into cityscapes convention class_lut = np.zeros([300, 1], dtype=np.uint8) class_lut[0] = 255 #unlabeled class_lut[1] = 255 #outlier class_lut[10] = 13 #car class_lut[11] = 13 #bike class_lut[13] = 13 #bus class_lut[15] = 13 #motorbike class_lut[16] = 13 #on rails class_lut[18] = 13 #truck class_lut[20] = 13 #other vehicle class_lut[30] = 255 #person class_lut[31] = 255 #cyclist class_lut[32] = 255 #motorcyclist class_lut[40] = 100 #road class_lut[44] = 100 #parking class_lut[48] = 102 #sidewalk class_lut[49] = 102 #other ground class_lut[50] = 2 #building class_lut[51] = 255 #fence class_lut[52] = 255 #other structure class_lut[60] = 100 #lane marking class_lut[70] = 7 #vegetation class_lut[71] = 7 #trunk class_lut[80] = 255 #pole class_lut[81] = 255 #traffic sign class_lut[99] = 255 #other object class_lut[252] = 100 #moving car class_lut[253] = 255 #moving cyclist class_lut[254] = 255 #moving person class_lut[255] = 255 #moving motorcyclist class_lut[256] = 13 #moving on rails class_lut[257] = 13 #moving bus class_lut[258] = 13 #moving truck class_lut[259] = 13 #moving other vehicle 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)) sem_data_dir = os.path.join(kitti.data_path, 'labels') sem_filenames = sorted(os.listdir(sem_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, sem_filenames) bar = progressbar.ProgressBar() ind = 0 for dt, filename, sem_filename in bar(iterable): if dt is None: continue velo_filename = os.path.join(velo_data_dir, filename) sem_filename = os.path.join(sem_data_dir, sem_filename) #read data scan = np.zeros([64*2048, 4], dtype=np.float32) scan_data = np.fromfile(velo_filename, dtype=np.float32).reshape(-1, 4) scan[:scan_data.shape[0], :] = scan_data sem = np.zeros([64*2048, 1], dtype=np.uint32) sem_data = np.fromfile(sem_filename, dtype=np.uint32).astype(np.uint16) sem[:sem_data.shape[0], :] = class_lut[sem_data] #print("sem max: " + str(np.max(sem))) #print("scan shape: " + str(scan.shape)) #print("sem shape: " + str(sem.shape)) # create header header = Header() header.frame_id = velo_frame_id header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f"))) header.stamp.nsecs = (header.stamp.nsecs//1000)*1000 header.seq = ind # fill pcl msg fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('rgb', 16, PointField.UINT32, 1)] sem_pcl_msg = pcl2.create_cloud(header, fields, np.hstack([scan[:, :-1], sem + sem*(2**8) + sem*(2**16)])) sem_pcl_msg.width = 2048 sem_pcl_msg.height = 64 sem_pcl_msg.row_step = sem_pcl_msg.width * sem_pcl_msg.point_step fields = [PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('intensity', 16, PointField.FLOAT32, 1), PointField('t', 20, PointField.UINT32, 1), PointField('reflectivity', 24, PointField.UINT16, 1), PointField('ring', 26, PointField.UINT8, 1), PointField('noise', 28, PointField.UINT16, 1), PointField('range', 32, PointField.UINT32, 1), PointField('pad4', 36, PointField.UINT32, 3)] space = np.zeros([scan.shape[0], 1], dtype=np.float32) pcl_msg = pcl2.create_cloud(header, fields, np.hstack([scan[:, :-1], scan[:, -1, None]*1000, space, space, space, space, (1000*np.linalg.norm(scan[:, :-1], axis=1)[:, None]).astype(np.int), space, space, space])) pcl_msg.width = 2048 pcl_msg.height = 64 pcl_msg.row_step = pcl_msg.width * pcl_msg.point_step ind += 1 bag.write(topic + '/pointcloud_sem', sem_pcl_msg, t=pcl_msg.header.stamp) bag.write(topic + '/pointcloud', pcl_msg, t=pcl_msg.header.stamp) #write semantic pano pano = np.zeros([64, 2048, 3], dtype=np.uint8) pano[:, :, 0] = sem.reshape(64, 2048) pano[:, :, 1] = sem.reshape(64, 2048) pano[:, :, 2] = sem.reshape(64, 2048) image_message = bridge.cv2_to_imgmsg(pano, encoding='bgr8') image_message.header = header bag.write(topic + '/semantic_pano', image_message, t = image_message.header.stamp)
def main(): # Select Scene: bag_ID = '024' # Create empty bag: bag_name = '/home/user/workspace/Datasets/Pandaset/RosBags/pandaset_' + \ str(bag_ID) + '.bag' bag = rosbag.Bag(bag_name, 'w', compression=rosbag.Compression.NONE) # Set frames: velo_frame_id = 'pandar' velo_topic = '/mytopic/' # Load the sequences: dataset = DataSet('/home/user/workspace/Datasets/Pandaset/dataset/') sequences = dataset.sequences(with_semseg=True) sequences.sort() seqence = dataset[bag_ID] seqence.load_lidar().load_semseg() # Time hack, get every 100ms (since 10Hz sensor) time = [] for res in perdelta(datetime.now(), datetime.now() + timedelta(minutes=1), timedelta(milliseconds=100)): time.append(res) print(len(time)) # Load Annotations: if you only want some to see some of the classes # annotations = [] # for seq in seqence.semseg: # ann = seq.to_numpy() # for index in range(0, len(ann)): # if(ann[index] == 1 or ann[index] == 2 or ann[index] == 3): # ann[index] = 1 # else: # ann[index] = 0 # annotations.append(ann) # Load Annotations: if you want to see all the classes # annotations = [] # for seq in seqence.semseg: # ann = seq.to_numpy() # annotations.append(ann) # Loas scans: i = 0 seqence.lidar.set_sensor(0) for scan in seqence.lidar: data = scan[['x', 'y', 'z', 'i']].to_numpy( dtype=np.float32).reshape(-1, 4) #full_data = np.hstack((data, annotations[i])) header = Header() header.frame_id = velo_frame_id header.stamp = rospy.Time.from_sec( float(datetime.strftime(time[i], "%s.%f"))) 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, data) bag.write(velo_topic + '/pointcloud', pcl_msg, t=pcl_msg.header.stamp) print(i) i += 1 print("## OVERVIEW ##") print(bag) bag.close()
sys.path.append('/usr/local/python') framenum = 0 # Parameters for OpenPose. Take a look at C++ OpenPose example for meaning of components. Ensure all below are filled try: from openpose import * except: raise Exception( 'Error: OpenPose library could not be found. Did you enable `BUILD_PYTHON` in CMake and have this Python script in the right folder?' ) # 点の座標を定義するフレームの名前 # PointCloud2のフィールドの一覧 FIELDS = [ # 点の座標(x, y, z) 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), # 点の色(RGB) # 赤: 0xff0000, 緑:0x00ff00, 青: 0x0000ff PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1), # 独自に定義したフィールド ## 何番目の人か PointField(name='person_num', offset=16, datatype=PointField.FLOAT32, count=1), ## 関節番号 PointField(name='joint_num', offset=20,