Python msg.PointCloud2方法代码示例

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def ros_to_pcl(ros_cloud):
    """ Converts a ROS PointCloud2 message to a pcl PointXYZRGB

        Args:
            ros_cloud (PointCloud2): ROS PointCloud2 message

        Returns:
            pcl.PointCloud_PointXYZRGB: PCL XYZRGB point cloud
    """
    points_list = []

    for data in pc2.read_points(ros_cloud, skip_nans=True):
        points_list.append([data[0], data[1], data[2], data[3]])

    pcl_data = pcl.PointCloud_PointXYZRGB()
    pcl_data.from_list(points_list)

    return pcl_data 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def array_to_pointcloud2(cloud_arr, stamp=None, frame_id=None):
    '''Converts a numpy record array to a sensor_msgs.msg.PointCloud2.
    '''
    # make it 2d (even if height will be 1)
    cloud_arr = np.atleast_2d(cloud_arr)

    cloud_msg = PointCloud2()

    if stamp is not None:
        cloud_msg.header.stamp = stamp
    if frame_id is not None:
        cloud_msg.header.frame_id = frame_id
    cloud_msg.height = cloud_arr.shape[0]
    cloud_msg.width = cloud_arr.shape[1]
    cloud_msg.fields = dtype_to_fields(cloud_arr.dtype)
    cloud_msg.is_bigendian = False # assumption
    cloud_msg.point_step = cloud_arr.dtype.itemsize
    cloud_msg.row_step = cloud_msg.point_step*cloud_arr.shape[1]
    cloud_msg.is_dense = all([np.isfinite(cloud_arr[fname]).all() for fname in cloud_arr.dtype.names])
    cloud_msg.data = cloud_arr.tostring()
    return cloud_msg 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def pointcloud2_to_array(cloud_msg, squeeze=True):
    ''' Converts a rospy PointCloud2 message to a numpy recordarray

    Reshapes the returned array to have shape (height, width), even if the height is 1.

    The reason for using np.fromstring rather than struct.unpack is speed... especially
    for large point clouds, this will be <much> faster.
    '''
    # construct a numpy record type equivalent to the point type of this cloud
    dtype_list = fields_to_dtype(cloud_msg.fields, cloud_msg.point_step)

    # parse the cloud into an array
    cloud_arr = np.fromstring(cloud_msg.data, dtype_list)

    # remove the dummy fields that were added
    cloud_arr = cloud_arr[
        [fname for fname, _type in dtype_list if not (fname[:len(DUMMY_FIELD_PREFIX)] == DUMMY_FIELD_PREFIX)]]

    if squeeze and cloud_msg.height == 1:
        return np.reshape(cloud_arr, (cloud_msg.width,))
    else:
        return np.reshape(cloud_arr, (cloud_msg.height, cloud_msg.width)) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def __init__(self):
        print "dataRecorder"
        self.record = False
        self.twist = None
        self.twistLock = Lock()
        self.bridge = CvBridge()
        self.globaltime = None
        self.controller = XBox360()
        ##rospy.Subscriber("/camera/depth/points" , PointCloud2, self.ptcloudCB)
        rospy.Subscriber("/camera/depth/image_rect_raw_drop", Image, self.depthCB)
        rospy.Subscriber("/camera/rgb/image_rect_color_drop", Image, self.streamCB)
        #rospy.Subscriber("/camera/rgb/image_rect_mono_drop", Image, self.streamCB)  ##for black and white images see run.launch and look at the drop fps nodes near the bottom
        rospy.Subscriber("/lidargrid", Image, self.lidargridCB)
        rospy.Subscriber("/cmd_vel", TwistStamped, self.cmd_velCB)
        rospy.Subscriber("/joy", Joy ,self.joyCB)
        self.sound = rospy.Publisher("/sound_server/speech_synth", String, queue_size=1)
        rospy.init_node('dataRecorder',anonymous=True)
        rospy.spin() 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def publish_pc2(pc, obj):
    """Publisher of PointCloud data"""
    pub = rospy.Publisher("/points_raw", PointCloud2, queue_size=1000000)
    rospy.init_node("pc2_publisher")
    header = std_msgs.msg.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = "velodyne"
    points = pc2.create_cloud_xyz32(header, pc[:, :3])

    pub2 = rospy.Publisher("/points_raw1", PointCloud2, queue_size=1000000)
    header = std_msgs.msg.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = "velodyne"
    points2 = pc2.create_cloud_xyz32(header, obj)

    r = rospy.Rate(0.1)
    while not rospy.is_shutdown():
        pub.publish(points)
        pub2.publish(points2)
        r.sleep() 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def pointcloud2_to_array(cloud_msg, split_rgb=False):
    """
    Converts a rospy PointCloud2 message to a numpy recordarray
    
    Reshapes the returned array to have shape (height, width), even if the height is 1.

    The reason for using np.fromstring rather than struct.unpack is speed... especially
    for large point clouds, this will be <much> faster.
    """
    # construct a numpy record type equivalent to the point type of this cloud
    dtype_list = pointcloud2_to_dtype(cloud_msg)

    # parse the cloud into an array
    cloud_arr = np.fromstring(cloud_msg.data, dtype_list)

    # remove the dummy fields that were added
    cloud_arr = cloud_arr[
        [fname for fname, _type in dtype_list if not (fname[:len(DUMMY_FIELD_PREFIX)] == DUMMY_FIELD_PREFIX)]]

    if split_rgb:
        cloud_arr = split_rgb_field(cloud_arr)
    
    return np.reshape(cloud_arr, (cloud_msg.height, cloud_msg.width)) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def array_to_pointcloud2(cloud_arr, stamp=None, frame_id=None, merge_rgb=False):
    """Converts a numpy record array to a sensor_msgs.msg.PointCloud2.
    """
    if merge_rgb:
        cloud_arr = merge_rgb_fields(cloud_arr)

    # make it 2d (even if height will be 1)
    cloud_arr = np.atleast_2d(cloud_arr)

    cloud_msg = PointCloud2()

    if stamp is not None:
        cloud_msg.header.stamp = stamp
    if frame_id is not None:
        cloud_msg.header.frame_id = frame_id
    cloud_msg.height = cloud_arr.shape[0]
    cloud_msg.width = cloud_arr.shape[1]
    cloud_msg.fields = arr_to_fields(cloud_arr)
    cloud_msg.is_bigendian = False  # assumption
    cloud_msg.point_step = cloud_arr.dtype.itemsize
    cloud_msg.row_step = cloud_msg.point_step*cloud_arr.shape[1]
    cloud_msg.is_dense = all([np.isfinite(cloud_arr[fname]).all() for fname in cloud_arr.dtype.names])
    cloud_msg.data = cloud_arr.tostring()
    return cloud_msg 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def pointcloud2_to_array(cloud_msg, squeeze=True):
    ''' Converts a rospy PointCloud2 message to a numpy recordarray 
    
    Reshapes the returned array to have shape (height, width), even if the height is 1.

    The reason for using np.frombuffer rather than struct.unpack is speed... especially
    for large point clouds, this will be <much> faster.
    '''
    # construct a numpy record type equivalent to the point type of this cloud
    dtype_list = fields_to_dtype(cloud_msg.fields, cloud_msg.point_step)

    # parse the cloud into an array
    cloud_arr = np.frombuffer(cloud_msg.data, dtype_list)

    # remove the dummy fields that were added
    cloud_arr = cloud_arr[
        [fname for fname, _type in dtype_list if not (fname[:len(DUMMY_FIELD_PREFIX)] == DUMMY_FIELD_PREFIX)]]
    
    if squeeze and cloud_msg.height == 1:
        return np.reshape(cloud_arr, (cloud_msg.width,))
    else:
        return np.reshape(cloud_arr, (cloud_msg.height, cloud_msg.width)) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def xyzrgb_array_to_pointcloud2(points, colors, stamp=None, frame_id=None, seq=None):
    '''
    Create a sensor_msgs.PointCloud2 from an array
    of points.
    '''
    msg = PointCloud2()
    assert(points.shape == colors.shape)

    buf = []

    if stamp:
        msg.header.stamp = stamp
    if frame_id:
        msg.header.frame_id = frame_id
    if seq:
        msg.header.seq = seq
    if len(points.shape) == 3:
        msg.height = points.shape[1]
        msg.width = points.shape[0]
    else:
        N = len(points)
        xyzrgb = np.array(np.hstack([points, colors]), dtype=np.float32)
        msg.height = 1
        msg.width = N

    msg.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)
    ]
    msg.is_bigendian = False
    msg.point_step = 24
    msg.row_step = msg.point_step * N
    msg.is_dense = True;
    msg.data = xyzrgb.tostring()

    return msg 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def raw_to_voxel(pc, resolution=0.50, x=(0, 90), y=(-50, 50), z=(-4.5, 5.5)):
    """Convert PointCloud2 to Voxel"""
    logic_x = np.logical_and(pc[:, 0] >= x[0], pc[:, 0] < x[1])
    logic_y = np.logical_and(pc[:, 1] >= y[0], pc[:, 1] < y[1])
    logic_z = np.logical_and(pc[:, 2] >= z[0], pc[:, 2] < z[1])
    pc = pc[:, :3][np.logical_and(logic_x, np.logical_and(logic_y, logic_z))]
    pc =((pc - np.array([x[0], y[0], z[0]])) / resolution).astype(np.int32)
    voxel = np.zeros((int((x[1] - x[0]) / resolution), int((y[1] - y[0]) / resolution), int(round((z[1]-z[0]) / resolution))))
    voxel[pc[:, 0], pc[:, 1], pc[:, 2]] = 1
    return voxel 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def capture_sample():
    """ Captures a PointCloud2 using the sensor stick RGBD camera

        Args: None

        Returns:
            PointCloud2: a single point cloud from the RGBD camrea
    """
    get_model_state_prox = rospy.ServiceProxy('gazebo/get_model_state',GetModelState)
    model_state = get_model_state_prox('training_model','world')

    set_model_state_prox = rospy.ServiceProxy('gazebo/set_model_state', SetModelState)

    roll = random.uniform(0, 2*math.pi)
    pitch = random.uniform(0, 2*math.pi)
    yaw = random.uniform(0, 2*math.pi)
    quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
    model_state.pose.orientation.x = quaternion[0]
    model_state.pose.orientation.y = quaternion[1]
    model_state.pose.orientation.z = quaternion[2]
    model_state.pose.orientation.w = quaternion[3]
    sms_req = SetModelStateRequest()
    sms_req.model_state.pose = model_state.pose
    sms_req.model_state.twist = model_state.twist
    sms_req.model_state.model_name = 'training_model'
    sms_req.model_state.reference_frame = 'world'
    set_model_state_prox(sms_req)

    return rospy.wait_for_message('/sensor_stick/point_cloud', PointCloud2) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def subscribe(self):
        topic = "object_tracker/blob_info"
        self.centroid_sub = rospy.Subscriber(topic, BlobInfoArray, self.centroid_callback)
        self.pc_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.pc_callback)
        self.info_sub = rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.info_callback) 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def publish_pc2(pc, obj, pre_obj=None, pc_1=None):
    """Publisher of PointCloud data"""
    pub = rospy.Publisher("/points_raw", PointCloud2, queue_size=10)
    rospy.init_node("pc2_publisher")
    header = std_msgs.msg.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = "velodyne"
    points = pc2.create_cloud_xyz32(header, pc[:, :3])

    pub2 = rospy.Publisher("/points_raw1", PointCloud2, queue_size=10)
    header = std_msgs.msg.Header()
    header.stamp = rospy.Time.now()
    header.frame_id = "velodyne"
    points2 = pc2.create_cloud_xyz32(header, obj)

    if pre_obj is not None:
        pub3 = rospy.Publisher("/points_raw2", PointCloud2, queue_size=10)
        header = std_msgs.msg.Header()
        header.stamp = rospy.Time.now()
        header.frame_id = "velodyne"
        points3 = pc2.create_cloud_xyz32(header, pre_obj)

    if pc_1 is not None:
        pub4 = rospy.Publisher("/points_raw3", PointCloud2, queue_size=10)
        header = std_msgs.msg.Header()
        header.stamp = rospy.Time.now()
        header.frame_id = "velodyne"
        points4 = pc2.create_cloud_xyz32(header, pc_1[:, :3])

    r = rospy.Rate(0.1)
    while not rospy.is_shutdown():
        pub.publish(points)
        pub2.publish(points2)
        if pre_obj is not None:
            pub3.publish(points3)
        if pc_1 is not None:
            pub4.publish(points4)
        r.sleep() 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def _cameraCb(self, msg):
        '''
        Parse points and fields from a pointcloud2 message. For more info:
          https://answers.ros.org/question/208834/read-colours-from-a-pointcloud2-python/

        Parameters:
        -----------
        msg: ros sensor_msgs/PointCloud2 message
        '''
        gen = pc2.read_points(msg, skip_nans=True)
        print(gen)
        raise RuntimeError('deprecated')
        pass 

# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import PointCloud2 [as 别名]
def run(self):
        rospy.init_node(self.config.name, anonymous=True, disable_signals=True)
        rospy.Subscriber(self._topic_name, PointCloud2, self.on_point_cloud)
        rospy.spin()