ROS中常用Message介绍与VINS轨迹数据保存

Apr 17,2021   36256 words   130 min

Tags: SLAM

0.背景

最近在跑SLAM的时候遇到一个不算复杂但是比较常见的问题。就是如何将一些SLAM(如VINS-Fusion)输出的ROS Topic格式的位姿保存成特定格式(如KITTI格式、TUM格式)的文本文件,便于之后进行进一步的定量分析与对比。因此需要研究如何将特定的一些ROS Topic的Messge保存成文本文件。这也就是本篇博客的主要内容。话不多说,直接开始。

1.常见Message介绍

众所周知,在ROS中有非常多的已经定义好的Msg类型,和SLAM相关的比如有:geometry_msgs,nav_msgs,sensor_msgs。当然这些是大类,每个里面又有很多更细节的Msg类型。因为类型太多了,也不可能每一个都学会。常用的了解了就可以,遇到一些不常用的,去ROS官网查一下API就可以了。这里就简单列举几个常用的类型以及它们各自的组成。如果不知道Message是什么,参考之前的这篇博客

(1) geometry_msgs

geometry_msgs是ROS中一类与几何相关的消息,包含常见的PointQuaternionTwistVector3Pose等类型,其包含的全部类型具体列举如下。

Accel
    Vector3 linear
    Vector3 angular

AccelStamped
    Header header
    Accel accel

AccelWithCovariance
    Accel accel
    # Row-major representation of the 6x6 covariance matrix
    # The orientation parameters use a fixed-axis representation.
    # In order, the parameters are:
    # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
    float[64] covariance

AccelWithCovarianceStamped
    Header header
    AccelWithCovariance accel


Inertia
    # Mass [kg]
    float64 m

    # Center of mass [m]
    geometry_msgs/Vector3 com

    # Inertia Tensor [kg-m^2]
    #     | ixx ixy ixz |
    # I = | ixy iyy iyz |
    #     | ixz iyz izz |
    float64 ixx
    float64 ixy
    float64 ixz
    float64 iyy
    float64 iyz
    float64 izz

InertiaStamped
    Header header
    Inertia inertia

Point
    float64 x
    float64 y
    float64 z

Point32
    float32 x
    float32 y
    float32 z

PointStamped
    Header header
    Point point

Polygon
    Point32[] points

PolygonStamped
    Header header
    Polygon polygon

Pose
    Point position
    Quaternion orientation

Pose2D
    # This expresses a position and orientation on a 2D manifold.
    float64 x
    float64 y
    float64 theta

PoseArray
    Header header
    Pose[] poses

PoseStamped
    Header header
    Pose pose

PoseWithCovariance
    Pose pose
    # Row-major representation of the 6x6 covariance matrix
    # The orientation parameters use a fixed-axis representation.
    # In order, the parameters are:
    # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
    float64[36] covariance

PoseWithCovarianceStamped
    Header header
    PoseWithCovariance pose

Quaternion
    float64 x
    float64 y
    float64 z
    float64 w

QuaternionStamped
    Header header
    Quaternion quaternion

Transform
    Vector3 translation
    Quaternion rotation

TransformStamped
    Header header
    # the frame id of the child frame
    string child_frame_id
    Transform transform

Twist
    # This expresses velocity in free space broken into its linear and angular parts.
    Vector3  linear
    Vector3  angular

TwistStamped
    # A twist with reference coordinate frame and timestamp
    Header header
    Twist twist

TwistWithCovariance
    # This expresses velocity in free space with uncertainty.
    Twist twist

    # Row-major representation of the 6x6 covariance matrix
    # The orientation parameters use a fixed-axis representation.
    # In order, the parameters are:
    # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
    float64[36] covariance

TwistWithCovarianceStamped
    Header header
    TwistWithCovariance twist

Vector3
    float64 x
    float64 y
    float64 z

Vector3Stamped
    Header header
    Vector3 vector

Wrench
    # This represents force in free space, separated into
    # its linear and angular parts.
    Vector3  force
    Vector3  torque

WrenchStamped
    Header header
    Wrench wrench
(2) nav_msgs

nav_msgs顾名思义是一些与导航有关的消息类型,对于SLAM而言,这里面就有一些相对常用的类型,如OdometryPath,具体包含如下类别:

GridCells
    #an array of cells in a 2D grid
    Header header
    float32 cell_width
    float32 cell_height
    geometry_msgs/Point[] cells

MapMetaData
    # This hold basic information about the characterists of the OccupancyGrid

    # The time at which the map was loaded
    time map_load_time
    # The map resolution [m/cell]
    float32 resolution
    # Map width [cells]
    uint32 width
    # Map height [cells]
    uint32 height
    # The origin of the map [m, m, rad].  This is the real-world pose of the
    # cell (0,0) in the map.
    geometry_msgs/Pose origin

OccupancyGrid
    # This represents a 2-D grid map, in which each cell represents the probability of
    # occupancy.

    Header header 

    #MetaData for the map
    MapMetaData info

    # The map data, in row-major order, starting with (0,0).  Occupancy
    # probabilities are in the range [0,100].  Unknown is -1.
    int8[] data

Odometry
    # This represents an estimate of a position and velocity in free space.  
    # The pose in this message should be specified in the coordinate frame given by header.frame_id.
    # The twist in this message should be specified in the coordinate frame given by the child_frame_id
    Header header
    string child_frame_id
    geometry_msgs/PoseWithCovariance pose
    geometry_msgs/TwistWithCovariance twist

Path
    #An array of poses that represents a Path for a robot to follow
    Header header
    geometry_msgs/PoseStamped[] poses
(3) sensor_msgs

同理,sensoe_msgs就是一系列和传感器有关的消息,一些常见的如ImageIMUPointCloud类型都属于它。当然,因为传感器种类繁多,因此这里会有很多我们几乎从没见过的类型,但也列出来了,作为了解即可,需要的时候可供查阅。所包含的全部类型罗列如下。

BatteryState
    # Constants are chosen to match the enums in the linux kernel
    # defined in include/linux/power_supply.h as of version 3.7
    # The one difference is for style reasons the constants are
    # all uppercase not mixed case.

    # Power supply status constants
    uint8 POWER_SUPPLY_STATUS_UNKNOWN = 0
    uint8 POWER_SUPPLY_STATUS_CHARGING = 1
    uint8 POWER_SUPPLY_STATUS_DISCHARGING = 2
    uint8 POWER_SUPPLY_STATUS_NOT_CHARGING = 3
    uint8 POWER_SUPPLY_STATUS_FULL = 4

    # Power supply health constants
    uint8 POWER_SUPPLY_HEALTH_UNKNOWN = 0
    uint8 POWER_SUPPLY_HEALTH_GOOD = 1
    uint8 POWER_SUPPLY_HEALTH_OVERHEAT = 2
    uint8 POWER_SUPPLY_HEALTH_DEAD = 3
    uint8 POWER_SUPPLY_HEALTH_OVERVOLTAGE = 4
    uint8 POWER_SUPPLY_HEALTH_UNSPEC_FAILURE = 5
    uint8 POWER_SUPPLY_HEALTH_COLD = 6
    uint8 POWER_SUPPLY_HEALTH_WATCHDOG_TIMER_EXPIRE = 7
    uint8 POWER_SUPPLY_HEALTH_SAFETY_TIMER_EXPIRE = 8

    # Power supply technology (chemistry) constants
    uint8 POWER_SUPPLY_TECHNOLOGY_UNKNOWN = 0
    uint8 POWER_SUPPLY_TECHNOLOGY_NIMH = 1
    uint8 POWER_SUPPLY_TECHNOLOGY_LION = 2
    uint8 POWER_SUPPLY_TECHNOLOGY_LIPO = 3
    uint8 POWER_SUPPLY_TECHNOLOGY_LIFE = 4
    uint8 POWER_SUPPLY_TECHNOLOGY_NICD = 5
    uint8 POWER_SUPPLY_TECHNOLOGY_LIMN = 6

    Header  header
    float32 voltage          # Voltage in Volts (Mandatory)
    float32 temperature      # Temperature in Degrees Celsius (If unmeasured NaN)
    float32 current          # Negative when discharging (A)  (If unmeasured NaN)
    float32 charge           # Current charge in Ah  (If unmeasured NaN)
    float32 capacity         # Capacity in Ah (last full capacity)  (If unmeasured NaN)
    float32 design_capacity  # Capacity in Ah (design capacity)  (If unmeasured NaN)
    float32 percentage       # Charge percentage on 0 to 1 range  (If unmeasured NaN)
    uint8   power_supply_status     # The charging status as reported. Values defined above
    uint8   power_supply_health     # The battery health metric. Values defined above
    uint8   power_supply_technology # The battery chemistry. Values defined above
    bool    present          # True if the battery is present

    float32[] cell_voltage   # An array of individual cell voltages for each cell in the pack
                            # If individual voltages unknown but number of cells known set each to NaN
    float32[] cell_temperature  # An array of individual cell temperatures for each cell in the pack
                                # If individual temperatures unknown but number of cells known set each to NaN
    string location          # The location into which the battery is inserted. (slot number or plug)
    string serial_number     # The best approximation of the battery serial number

CameraInfo
    # This message defines meta information for a camera. It should be in a
    # camera namespace on topic "camera_info" and accompanied by up to five
    # image topics named:
    #
    #   image_raw - raw data from the camera driver, possibly Bayer encoded
    #   image            - monochrome, distorted
    #   image_color      - color, distorted
    #   image_rect       - monochrome, rectified
    #   image_rect_color - color, rectified
    #
    # The image_pipeline contains packages (image_proc, stereo_image_proc)
    # for producing the four processed image topics from image_raw and
    # camera_info. The meaning of the camera parameters are described in
    # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
    #
    # The image_geometry package provides a user-friendly interface to
    # common operations using this meta information. If you want to, e.g.,
    # project a 3d point into image coordinates, we strongly recommend
    # using image_geometry.
    #
    # If the camera is uncalibrated, the matrices D, K, R, P should be left
    # zeroed out. In particular, clients may assume that K[0] == 0.0
    # indicates an uncalibrated camera.

    #######################################################################
    #                     Image acquisition info                          #
    #######################################################################

    # Time of image acquisition, camera coordinate frame ID
    Header header    # Header timestamp should be acquisition time of image
                    # Header frame_id should be optical frame of camera
                    # origin of frame should be optical center of camera
                    # +x should point to the right in the image
                    # +y should point down in the image
                    # +z should point into the plane of the image


    #######################################################################
    #                      Calibration Parameters                         #
    #######################################################################
    # These are fixed during camera calibration. Their values will be the #
    # same in all messages until the camera is recalibrated. Note that    #
    # self-calibrating systems may "recalibrate" frequently.              #
    #                                                                     #
    # The internal parameters can be used to warp a raw (distorted) image #
    # to:                                                                 #
    #   1. An undistorted image (requires D and K)                        #
    #   2. A rectified image (requires D, K, R)                           #
    # The projection matrix P projects 3D points into the rectified image.#
    #######################################################################

    # The image dimensions with which the camera was calibrated. Normally
    # this will be the full camera resolution in pixels.
    uint32 height
    uint32 width

    # The distortion model used. Supported models are listed in
    # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
    # simple model of radial and tangential distortion - is sufficient.
    string distortion_model

    # The distortion parameters, size depending on the distortion model.
    # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
    float64[] D

    # Intrinsic camera matrix for the raw (distorted) images.
    #     [fx  0 cx]
    # K = [ 0 fy cy]
    #     [ 0  0  1]
    # Projects 3D points in the camera coordinate frame to 2D pixel
    # coordinates using the focal lengths (fx, fy) and principal point
    # (cx, cy).
    float64[9]  K # 3x3 row-major matrix

    # Rectification matrix (stereo cameras only)
    # A rotation matrix aligning the camera coordinate system to the ideal
    # stereo image plane so that epipolar lines in both stereo images are
    # parallel.
    float64[9]  R # 3x3 row-major matrix

    # Projection/camera matrix
    #     [fx'  0  cx' Tx]
    # P = [ 0  fy' cy' Ty]
    #     [ 0   0   1   0]
    # By convention, this matrix specifies the intrinsic (camera) matrix
    #  of the processed (rectified) image. That is, the left 3x3 portion
    #  is the normal camera intrinsic matrix for the rectified image.
    # It projects 3D points in the camera coordinate frame to 2D pixel
    #  coordinates using the focal lengths (fx', fy') and principal point
    #  (cx', cy') - these may differ from the values in K.
    # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
    #  also have R = the identity and P[1:3,1:3] = K.
    # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
    #  position of the optical center of the second camera in the first
    #  camera's frame. We assume Tz = 0 so both cameras are in the same
    #  stereo image plane. The first camera always has Tx = Ty = 0. For
    #  the right (second) camera of a horizontal stereo pair, Ty = 0 and
    #  Tx = -fx' * B, where B is the baseline between the cameras.
    # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
    #  the rectified image is given by:
    #  [u v w]' = P * [X Y Z 1]'
    #         x = u / w
    #         y = v / w
    #  This holds for both images of a stereo pair.
    float64[12] P # 3x4 row-major matrix


    #######################################################################
    #                      Operational Parameters                         #
    #######################################################################
    # These define the image region actually captured by the camera       #
    # driver. Although they affect the geometry of the output image, they #
    # may be changed freely without recalibrating the camera.             #
    #######################################################################

    # Binning refers here to any camera setting which combines rectangular
    #  neighborhoods of pixels into larger "super-pixels." It reduces the
    #  resolution of the output image to
    #  (width / binning_x) x (height / binning_y).
    # The default values binning_x = binning_y = 0 is considered the same
    #  as binning_x = binning_y = 1 (no subsampling).
    uint32 binning_x
    uint32 binning_y

    # Region of interest (subwindow of full camera resolution), given in
    #  full resolution (unbinned) image coordinates. A particular ROI
    #  always denotes the same window of pixels on the camera sensor,
    #  regardless of binning settings.
    # The default setting of roi (all values 0) is considered the same as
    #  full resolution (roi.width = width, roi.height = height).
    RegionOfInterest roi


ChannelFloat32
    # This message is used by the PointCloud message to hold optional data
    # associated with each point in the cloud. The length of the values
    # array should be the same as the length of the points array in the
    # PointCloud, and each value should be associated with the corresponding
    # point.

    # Channel names in existing practice include:
    #   "u", "v" - row and column (respectively) in the left stereo image.
    #              This is opposite to usual conventions but remains for
    #              historical reasons. The newer PointCloud2 message has no
    #              such problem.
    #   "rgb" - For point clouds produced by color stereo cameras. uint8
    #           (R,G,B) values packed into the least significant 24 bits,
    #           in order.
    #   "intensity" - laser or pixel intensity.
    #   "distance"

    # The channel name should give semantics of the channel (e.g.
    # "intensity" instead of "value").
    string name

    # The values array should be 1-1 with the elements of the associated
    # PointCloud.
    float32[] values

CompressedImage
    # This message contains a compressed image

    Header header        # Header timestamp should be acquisition time of image
                        # Header frame_id should be optical frame of camera
                        # origin of frame should be optical center of camera
                        # +x should point to the right in the image
                        # +y should point down in the image
                        # +z should point into to plane of the image

    string format        # Specifies the format of the data
                        #   Acceptable values:
                        #     jpeg, png
    uint8[] data         # Compressed image buffer

FluidPressure
    # Single pressure reading.  This message is appropriate for measuring the
    # pressure inside of a fluid (air, water, etc).  This also includes
    # atmospheric or barometric pressure.

    # This message is not appropriate for force/pressure contact sensors.

    Header header           # timestamp of the measurement
                            # frame_id is the location of the pressure sensor

    float64 fluid_pressure  # Absolute pressure reading in Pascals.

    float64 variance        # 0 is interpreted as variance unknown

Illuminance
    # Single photometric illuminance measurement.  Light should be assumed to be
    # measured along the sensor's x-axis (the area of detection is the y-z plane).
    # The illuminance should have a 0 or positive value and be received with
    # the sensor's +X axis pointing toward the light source.

    # Photometric illuminance is the measure of the human eye's sensitivity of the
    # intensity of light encountering or passing through a surface.

    # All other Photometric and Radiometric measurements should
    # not use this message.
    # This message cannot represent:
    # Luminous intensity (candela/light source output)
    # Luminance (nits/light output per area)
    # Irradiance (watt/area), etc.

    Header header           # timestamp is the time the illuminance was measured
                            # frame_id is the location and direction of the reading

    float64 illuminance     # Measurement of the Photometric Illuminance in Lux.

    float64 variance        # 0 is interpreted as variance unknown

Image
    # This message contains an uncompressed image
    # (0, 0) is at top-left corner of image
    #

    Header header        # Header timestamp should be acquisition time of image
                        # Header frame_id should be optical frame of camera
                        # origin of frame should be optical center of camera
                        # +x should point to the right in the image
                        # +y should point down in the image
                        # +z should point into to plane of the image
                        # If the frame_id here and the frame_id of the CameraInfo
                        # message associated with the image conflict
                        # the behavior is undefined

    uint32 height         # image height, that is, number of rows
    uint32 width          # image width, that is, number of columns

    # The legal values for encoding are in file src/image_encodings.cpp
    # If you want to standardize a new string format, join
    # ros-users@lists.sourceforge.net and send an email proposing a new encoding.

    string encoding       # Encoding of pixels -- channel meaning, ordering, size
                        # taken from the list of strings in include/sensor_msgs/image_encodings.h

    uint8 is_bigendian    # is this data bigendian?
    uint32 step           # Full row length in bytes
    uint8[] data          # actual matrix data, size is (step * rows)

Imu
    # This is a message to hold data from an IMU (Inertial Measurement Unit)
    #
    # Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
    #
    # If the covariance of the measurement is known, it should be filled in (if all you know is the 
    # variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
    # A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
    # data a covariance will have to be assumed or gotten from some other source
    #
    # If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation 
    # estimate), please set element 0 of the associated covariance matrix to -1
    # If you are interpreting this message, please check for a value of -1 in the first element of each 
    # covariance matrix, and disregard the associated estimate.

    Header header

    geometry_msgs/Quaternion orientation
    float64[9] orientation_covariance # Row major about x, y, z axes

    geometry_msgs/Vector3 angular_velocity
    float64[9] angular_velocity_covariance # Row major about x, y, z axes

    geometry_msgs/Vector3 linear_acceleration
    float64[9] linear_acceleration_covariance # Row major x, y z 

JointState
    # This is a message that holds data to describe the state of a set of torque controlled joints. 
    #
    # The state of each joint (revolute or prismatic) is defined by:
    #  * the position of the joint (rad or m),
    #  * the velocity of the joint (rad/s or m/s) and 
    #  * the effort that is applied in the joint (Nm or N).
    #
    # Each joint is uniquely identified by its name
    # The header specifies the time at which the joint states were recorded. All the joint states
    # in one message have to be recorded at the same time.
    #
    # This message consists of a multiple arrays, one for each part of the joint state. 
    # The goal is to make each of the fields optional. When e.g. your joints have no
    # effort associated with them, you can leave the effort array empty. 
    #
    # All arrays in this message should have the same size, or be empty.
    # This is the only way to uniquely associate the joint name with the correct
    # states.
    Header header

    string[] name
    float64[] position
    float64[] velocity
    float64[] effort

Joy
    # Reports the state of a joysticks axes and buttons.
    Header header           # timestamp in the header is the time the data is received from the joystick
    float32[] axes          # the axes measurements from a joystick
    int32[] buttons         # the buttons measurements from a joystick 

JoyFeedback
    # Declare of the type of feedback
    uint8 TYPE_LED    = 0
    uint8 TYPE_RUMBLE = 1
    uint8 TYPE_BUZZER = 2

    uint8 type

    # This will hold an id number for each type of each feedback.
    # Example, the first led would be id=0, the second would be id=1
    uint8 id

    # Intensity of the feedback, from 0.0 to 1.0, inclusive.  If device is
    # actually binary, driver should treat 0<=x<0.5 as off, 0.5<=x<=1 as on.
    float32 intensity

JoyFeedbackArray
    # This message publishes values for multiple feedback at once. 
    JoyFeedback[] array

LaserEcho
    # This message is a submessage of MultiEchoLaserScan and is not intended
    # to be used separately.

    float32[] echoes  # Multiple values of ranges or intensities.
                    # Each array represents data from the same angle increment.

LaserScan
    # Single scan from a planar laser range-finder
    #
    # If you have another ranging device with different behavior (e.g. a sonar
    # array), please find or create a different message, since applications
    # will make fairly laser-specific assumptions about this data

    Header header            # timestamp in the header is the acquisition time of 
                            # the first ray in the scan.
                            #
                            # in frame frame_id, angles are measured around 
                            # the positive Z axis (counterclockwise, if Z is up)
                            # with zero angle being forward along the x axis
                            
    float32 angle_min        # start angle of the scan [rad]
    float32 angle_max        # end angle of the scan [rad]
    float32 angle_increment  # angular distance between measurements [rad]

    float32 time_increment   # time between measurements [seconds] - if your scanner
                            # is moving, this will be used in interpolating position
                            # of 3d points
    float32 scan_time        # time between scans [seconds]

    float32 range_min        # minimum range value [m]
    float32 range_max        # maximum range value [m]

    float32[] ranges         # range data [m] (Note: values < range_min or > range_max should be discarded)
    float32[] intensities    # intensity data [device-specific units].  If your
                            # device does not provide intensities, please leave
                            # the array empty.

MagneticField
    # Measurement of the Magnetic Field vector at a specific location.

    # If the covariance of the measurement is known, it should be filled in
    # (if all you know is the variance of each measurement, e.g. from the datasheet,
    #just put those along the diagonal)
    # A covariance matrix of all zeros will be interpreted as "covariance unknown",
    # and to use the data a covariance will have to be assumed or gotten from some
    # other source


    Header header                        # timestamp is the time the
                                        # field was measured
                                        # frame_id is the location and orientation
                                        # of the field measurement

    geometry_msgs/Vector3 magnetic_field # x, y, and z components of the
                                        # field vector in Tesla
                                        # If your sensor does not output 3 axes,
                                        # put NaNs in the components not reported.

    float64[9] magnetic_field_covariance # Row major about x, y, z axes
                                        # 0 is interpreted as variance unknown

MultiDOFJointState
    # Representation of state for joints with multiple degrees of freedom, 
    # following the structure of JointState.
    #
    # It is assumed that a joint in a system corresponds to a transform that gets applied 
    # along the kinematic chain. For example, a planar joint (as in URDF) is 3DOF (x, y, yaw)
    # and those 3DOF can be expressed as a transformation matrix, and that transformation
    # matrix can be converted back to (x, y, yaw)
    #
    # Each joint is uniquely identified by its name
    # The header specifies the time at which the joint states were recorded. All the joint states
    # in one message have to be recorded at the same time.
    #
    # This message consists of a multiple arrays, one for each part of the joint state. 
    # The goal is to make each of the fields optional. When e.g. your joints have no
    # wrench associated with them, you can leave the wrench array empty. 
    #
    # All arrays in this message should have the same size, or be empty.
    # This is the only way to uniquely associate the joint name with the correct
    # states.

    Header header

    string[] joint_names
    geometry_msgs/Transform[] transforms
    geometry_msgs/Twist[] twist
    geometry_msgs/Wrench[] wrench

MultiEchoLaserScan
    # Single scan from a multi-echo planar laser range-finder
    #
    # If you have another ranging device with different behavior (e.g. a sonar
    # array), please find or create a different message, since applications
    # will make fairly laser-specific assumptions about this data

    Header header            # timestamp in the header is the acquisition time of 
                            # the first ray in the scan.
                            #
                            # in frame frame_id, angles are measured around 
                            # the positive Z axis (counterclockwise, if Z is up)
                            # with zero angle being forward along the x axis
                            
    float32 angle_min        # start angle of the scan [rad]
    float32 angle_max        # end angle of the scan [rad]
    float32 angle_increment  # angular distance between measurements [rad]

    float32 time_increment   # time between measurements [seconds] - if your scanner
                            # is moving, this will be used in interpolating position
                            # of 3d points
    float32 scan_time        # time between scans [seconds]

    float32 range_min        # minimum range value [m]
    float32 range_max        # maximum range value [m]

    LaserEcho[] ranges       # range data [m] (Note: NaNs, values < range_min or > range_max should be discarded)
                            # +Inf measurements are out of range
                            # -Inf measurements are too close to determine exact distance.
    LaserEcho[] intensities  # intensity data [device-specific units].  If your
                            # device does not provide intensities, please leave
                            # the array empty.

NavSatFix
    # Navigation Satellite fix for any Global Navigation Satellite System
    #
    # Specified using the WGS 84 reference ellipsoid

    # header.stamp specifies the ROS time for this measurement (the
    #        corresponding satellite time may be reported using the
    #        sensor_msgs/TimeReference message).
    #
    # header.frame_id is the frame of reference reported by the satellite
    #        receiver, usually the location of the antenna.  This is a
    #        Euclidean frame relative to the vehicle, not a reference
    #        ellipsoid.
    Header header

    # satellite fix status information
    NavSatStatus status

    # Latitude [degrees]. Positive is north of equator; negative is south.
    float64 latitude

    # Longitude [degrees]. Positive is east of prime meridian; negative is west.
    float64 longitude

    # Altitude [m]. Positive is above the WGS 84 ellipsoid
    # (quiet NaN if no altitude is available).
    float64 altitude

    # Position covariance [m^2] defined relative to a tangential plane
    # through the reported position. The components are East, North, and
    # Up (ENU), in row-major order.
    #
    # Beware: this coordinate system exhibits singularities at the poles.

    float64[9] position_covariance

    # If the covariance of the fix is known, fill it in completely. If the
    # GPS receiver provides the variance of each measurement, put them
    # along the diagonal. If only Dilution of Precision is available,
    # estimate an approximate covariance from that.

    uint8 COVARIANCE_TYPE_UNKNOWN = 0
    uint8 COVARIANCE_TYPE_APPROXIMATED = 1
    uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
    uint8 COVARIANCE_TYPE_KNOWN = 3

    uint8 position_covariance_type

NavSatStatus
    # Navigation Satellite fix status for any Global Navigation Satellite System

    # Whether to output an augmented fix is determined by both the fix
    # type and the last time differential corrections were received.  A
    # fix is valid when status >= STATUS_FIX.

    int8 STATUS_NO_FIX =  -1        # unable to fix position
    int8 STATUS_FIX =      0        # unaugmented fix
    int8 STATUS_SBAS_FIX = 1        # with satellite-based augmentation
    int8 STATUS_GBAS_FIX = 2        # with ground-based augmentation

    int8 status

    # Bits defining which Global Navigation Satellite System signals were
    # used by the receiver.

    uint16 SERVICE_GPS =     1
    uint16 SERVICE_GLONASS = 2
    uint16 SERVICE_COMPASS = 4      # includes BeiDou.
    uint16 SERVICE_GALILEO = 8

    uint16 service

PointCloud
    # This message holds a collection of 3d points, plus optional additional
    # information about each point.

    # Time of sensor data acquisition, coordinate frame ID.
    Header header

    # Array of 3d points. Each Point32 should be interpreted as a 3d point
    # in the frame given in the header.
    geometry_msgs/Point32[] points

    # Each channel should have the same number of elements as points array,
    # and the data in each channel should correspond 1:1 with each point.
    # Channel names in common practice are listed in ChannelFloat32.msg.
    ChannelFloat32[] channels

PointCloud2
    # This message holds a collection of N-dimensional points, which may
    # contain additional information such as normals, intensity, etc. The
    # point data is stored as a binary blob, its layout described by the
    # contents of the "fields" array.

    # The point cloud data may be organized 2d (image-like) or 1d
    # (unordered). Point clouds organized as 2d images may be produced by
    # camera depth sensors such as stereo or time-of-flight.

    # Time of sensor data acquisition, and the coordinate frame ID (for 3d
    # points).
    Header header

    # 2D structure of the point cloud. If the cloud is unordered, height is
    # 1 and width is the length of the point cloud.
    uint32 height
    uint32 width

    # Describes the channels and their layout in the binary data blob.
    PointField[] fields

    bool    is_bigendian # Is this data bigendian?
    uint32  point_step   # Length of a point in bytes
    uint32  row_step     # Length of a row in bytes
    uint8[] data         # Actual point data, size is (row_step*height)

    bool is_dense        # True if there are no invalid points

PointField
    # This message holds the description of one point entry in the
    # PointCloud2 message format.
    uint8 INT8    = 1
    uint8 UINT8   = 2
    uint8 INT16   = 3
    uint8 UINT16  = 4
    uint8 INT32   = 5
    uint8 UINT32  = 6
    uint8 FLOAT32 = 7
    uint8 FLOAT64 = 8

    string name      # Name of field
    uint32 offset    # Offset from start of point struct
    uint8  datatype  # Datatype enumeration, see above
    uint32 count     # How many elements in the field

Range
    # Single range reading from an active ranger that emits energy and reports
    # one range reading that is valid along an arc at the distance measured. 
    # This message is  not appropriate for laser scanners. See the LaserScan
    # message if you are working with a laser scanner.

    # This message also can represent a fixed-distance (binary) ranger.  This
    # sensor will have min_range===max_range===distance of detection.
    # These sensors follow REP 117 and will output -Inf if the object is detected
    # and +Inf if the object is outside of the detection range.

    Header header           # timestamp in the header is the time the ranger
                            # returned the distance reading

    # Radiation type enums
    # If you want a value added to this list, send an email to the ros-users list
    uint8 ULTRASOUND=0
    uint8 INFRARED=1

    uint8 radiation_type    # the type of radiation used by the sensor
                            # (sound, IR, etc) [enum]

    float32 field_of_view   # the size of the arc that the distance reading is
                            # valid for [rad]
                            # the object causing the range reading may have
                            # been anywhere within -field_of_view/2 and
                            # field_of_view/2 at the measured range. 
                            # 0 angle corresponds to the x-axis of the sensor.

    float32 min_range       # minimum range value [m]
    float32 max_range       # maximum range value [m]
                            # Fixed distance rangers require min_range==max_range

    float32 range           # range data [m]
                            # (Note: values < range_min or > range_max
                            # should be discarded)
                            # Fixed distance rangers only output -Inf or +Inf.
                            # -Inf represents a detection within fixed distance.
                            # (Detection too close to the sensor to quantify)
                            # +Inf represents no detection within the fixed distance.
                            # (Object out of range)

RegionOfInterest
    # This message is used to specify a region of interest within an image.
    #
    # When used to specify the ROI setting of the camera when the image was
    # taken, the height and width fields should either match the height and
    # width fields for the associated image; or height = width = 0
    # indicates that the full resolution image was captured.

    uint32 x_offset  # Leftmost pixel of the ROI
                    # (0 if the ROI includes the left edge of the image)
    uint32 y_offset  # Topmost pixel of the ROI
                    # (0 if the ROI includes the top edge of the image)
    uint32 height    # Height of ROI
    uint32 width     # Width of ROI

    # True if a distinct rectified ROI should be calculated from the "raw"
    # ROI in this message. Typically this should be False if the full image
    # is captured (ROI not used), and True if a subwindow is captured (ROI
    # used).
    bool do_rectify

RelativeHumidity
    # Single reading from a relative humidity sensor.  Defines the ratio of partial
    # pressure of water vapor to the saturated vapor pressure at a temperature.

    Header header             # timestamp of the measurement
                            # frame_id is the location of the humidity sensor

    float64 relative_humidity # Expression of the relative humidity
                            # from 0.0 to 1.0.
                            # 0.0 is no partial pressure of water vapor
                            # 1.0 represents partial pressure of saturation

    float64 variance          # 0 is interpreted as variance unknown

Temperature
    # Single temperature reading.

    Header header           # timestamp is the time the temperature was measured
                            # frame_id is the location of the temperature reading

    float64 temperature     # Measurement of the Temperature in Degrees Celsius

    float64 variance        # 0 is interpreted as variance unknown

TimeReference
    # Measurement from an external time source not actively synchronized with the system clock.

    Header header    # stamp is system time for which measurement was valid
                    # frame_id is not used 

    time   time_ref  # corresponding time from this external source
    string source    # (optional) name of time source
(4) stereo_msgs

双目消息类型其实里面只有一个类型,即DisparityImage

DisparityImage
    # Separate header for compatibility with current TimeSynchronizer.
    # Likely to be removed in a later release, use image.header instead.
    Header header

    # Floating point disparity image. The disparities are pre-adjusted for any
    # x-offset between the principal points of the two cameras (in the case
    # that they are verged). That is: d = x_l - x_r - (cx_l - cx_r)
    sensor_msgs/Image image

    # Stereo geometry. For disparity d, the depth from the camera is Z = fT/d.
    float32 f # Focal length, pixels
    float32 T # Baseline, world units

    # Subwindow of (potentially) valid disparity values.
    sensor_msgs/RegionOfInterest valid_window

    # The range of disparities searched.
    # In the disparity image, any disparity less than min_disparity is invalid.
    # The disparity search range defines the horopter, or 3D volume that the
    # stereo algorithm can "see". Points with Z outside of:
    #     Z_min = fT / max_disparity
    #     Z_max = fT / min_disparity
    # could not be found.
    float32 min_disparity
    float32 max_disparity

    # Smallest allowed disparity increment. The smallest achievable depth range
    # resolution is delta_Z = (Z^2/fT)*delta_d.
    float32 delta_d
(5) std_msgs

另外再补充一下标准信息,上面反复出现的Header就在std_msgs里,它里面包含了一些非常基本的数据类型。

Bool
    bool data

Byte
    byte data

ByteMultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    byte[]            data          # array of data

Char
    char data

ColorRGBA
    float32 r
    float32 g
    float32 b
    float32 a

Duration
    duration data

Empty
    -

Float32
    float32 data

Float32MultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    float32[]         data          # array of data

Float64
    float64 data

Float64MultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    float64[]         data          # array of data

Header
    # Standard metadata for higher-level stamped data types.
    # This is generally used to communicate timestamped data 
    # in a particular coordinate frame.
    # 
    # sequence ID: consecutively increasing ID 
    uint32 seq
    #Two-integer timestamp that is expressed as:
    # * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
    # * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
    # time-handling sugar is provided by the client library
    time stamp
    #Frame this data is associated with
    string frame_id

Int16
    int16 data

Int16MultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    int16[]           data          # array of data

Int32
    int32 data

Int32MultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    int32[]           data          # array of data

Int64
    int64 data

Int64MultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    int64[]           data          # array of data

Int8
    int8 data

Int8MultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    int8[]            data          # array of data

MultiArrayDimension
    string label   # label of given dimension
    uint32 size    # size of given dimension (in type units)
    uint32 stride  # stride of given dimension

MultiArrayLayout
    # The multiarray declares a generic multi-dimensional array of a
    # particular data type.  Dimensions are ordered from outer most
    # to inner most.

    MultiArrayDimension[] dim # Array of dimension properties
    uint32 data_offset        # padding elements at front of data

    # Accessors should ALWAYS be written in terms of dimension stride
    # and specified outer-most dimension first.
    # 
    # multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]
    #
    # A standard, 3-channel 640x480 image with interleaved color channels
    # would be specified as:
    #
    # dim[0].label  = "height"
    # dim[0].size   = 480
    # dim[0].stride = 3*640*480 = 921600  (note dim[0] stride is just size of image)
    # dim[1].label  = "width"
    # dim[1].size   = 640
    # dim[1].stride = 3*640 = 1920
    # dim[2].label  = "channel"
    # dim[2].size   = 3
    # dim[2].stride = 3
    #
    # multiarray(i,j,k) refers to the ith row, jth column, and kth channel.

String
    string data

Time
    time data

UInt16
    uint16 data

UInt16MultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    uint16[]            data        # array of data

UInt32
    uint32 data

UInt32MultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    uint32[]          data          # array of data

UInt64
    uint64 data

UInt64MultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    uint64[]          data          # array of data

UInt8
    uint8 data

UInt8MultiArray
    # Please look at the MultiArrayLayout message definition for
    # documentation on all multiarrays.

    MultiArrayLayout  layout        # specification of data layout
    uint8[]           data          # array of data

2.消息实例

这里以VINS-Fusion发布的Topic为例,它发布了以下这些Topic。 进一步,我们可以看看它发布的Topic都是什么类型,可以用rostopic info topic_name查看具体的Topic信息。比如我们输入rostopic info /vins_estimator/camera_pose 可以看到,它发布的主要是Odometry类型。当然,也有发布的是Path类型的Topic。

3.保存Topic数据

(1) 订阅特定Topic

关于如何用代码编写节点订阅Topic可以参考这篇博客。简单来说如下所示。

# coding=utf-8
import sys
import rospy
from nav_msgs.msg import Odometry

def callback(Odometry):
    timestamp = Odometry.header.stamp
    pos = Odometry.pose.pose.position
    ori = Odometry.pose.pose.orientation

    pos_x = pos.x
    pos_y = pos.y
    pos_z = pos.z
    ori_x = ori.x
    ori_y = ori.y
    ori_z = ori.z
    ori_w = ori.w

    fout.write(str(timestamp)+" "+str(pos_x)+" "+str(pos_y)+" "+str(pos_z)+" "+str(ori_x)+" "+str(ori_y)+" "+str(ori_z)+" "+str(ori_w)+"\n")
    
    print("Save to file",timestamp)


if __name__ == '__main__':
    topic_name = sys.argv[1]
    out_path = sys.argv[2]
    subscriber_name = "subscriber_" + topic_name.split("/")[-1]

    fout = open(out_path,'a')

    print("waiting for message coming...")

    # 初始化节点
    rospy.init_node(subscriber_name)
    # 开始订阅
    rospy.Subscriber(topic_name, Odometry, callback)
    # 循环
    rospy.spin()
(2) 读取数据

按照上面介绍的Msg的格式来读就可以了,例如这里获取的Odometry类型,可以通过如下代码读取。

def callback(Odometry):
    timestamp = Odometry.header.stamp
    pos = Odometry.pose.pose.position
    ori = Odometry.pose.pose.orientation

    pos_x = pos.x
    pos_y = pos.y
    pos_z = pos.z
    ori_x = ori.x
    ori_y = ori.y
    ori_z = ori.z
    ori_w = ori.w

    fout.write(str(timestamp)+" "+str(pos_x)+" "+str(pos_y)+" "+str(pos_z)+" "+str(ori_x)+" "+str(ori_y)+" "+str(ori_z)+" "+str(ori_w)+"\n")
    
    print("Save to file",timestamp)
(3) 保存数据

保存数据本身是十分简单的事情,上面的代码也已经展示了。但唯一需要注意的是保存数据的格式。一般而言,比较常用的是KITTI和TUM两种数据格式。在ORB-SLAM中保存轨迹就是这两种格式。这里我们把ORB-SLAM3中保存轨迹的代码截取出来,看一下这两种格式都是什么样的。

// TUM数据格式
Trw = Trw*pKF->GetPose()*Two;
cv::Mat Tcw = (*lit)*Trw;
cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
vector<float> q = Converter::toQuaternion(Rwc);
f << setprecision(6) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;

// KITTI数据格式
Trw = Trw*pKF->GetPose()*Two;

cv::Mat Tcw = (*lit)*Trw;
cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);

f << setprecision(9) << Rwc.at<float>(0,0) << " " << Rwc.at<float>(0,1)  << " " << Rwc.at<float>(0,2) << " "  << twc.at<float>(0) << " " <<
        Rwc.at<float>(1,0) << " " << Rwc.at<float>(1,1)  << " " << Rwc.at<float>(1,2) << " "  << twc.at<float>(1) << " " <<
        Rwc.at<float>(2,0) << " " << Rwc.at<float>(2,1)  << " " << Rwc.at<float>(2,2) << " "  << twc.at<float>(2) << endl;

这其中四元数q的顺序十分重要,是x、y、z、w还是w、x、y、z。这个问题我们可以从toQuaternion()源码中找到答案。

std::vector<float> Converter::toQuaternion(const cv::Mat &M)
{
    Eigen::Matrix<double,3,3> eigMat = toMatrix3d(M);
    Eigen::Quaterniond q(eigMat);

    std::vector<float> v(4);
    v[0] = q.x();
    v[1] = q.y();
    v[2] = q.z();
    v[3] = q.w();

    return v;
}

可以清晰地看到,这里四元数的顺序是x、y、z、w。因此这里简单总结一下,TUM和KITTI的两种格式罗列如下。

  • TUM:timestamp、x、y、z、q_x、q_y、q_z、q_w。简单来说就是时间戳+平移+四元数

  • KITTI:R(0,0)、R(0,1)、R(0,2)、t(0)、R(1,0)、R(1,1)、R(1,2)、t(1)、R(2,0)、R(2,1)、R(2,2)、t(2)。简单来说就是平移矩阵T中除去最后一行0,0,0,1后的旋转矩阵部分R和平移t。

4.实际VINS测试

上面也说到VINS-Fusion会输出/vins_estimator/odometryTopic,我们可以订阅这个Topic并将Messages保存下来,即可实现对于轨迹的保存。保存Odometry类型数据的脚本如下。

# coding=utf-8
import sys
import rospy
from nav_msgs.msg import Odometry

def callback(Odometry):
    timestamp = Odometry.header.stamp
    pos = Odometry.pose.pose.position
    ori = Odometry.pose.pose.orientation

    pos_x = pos.x
    pos_y = pos.y
    pos_z = pos.z
    ori_x = ori.x
    ori_y = ori.y
    ori_z = ori.z
    ori_w = ori.w

    fout.write(str(timestamp)+" "+str(pos_x)+" "+str(pos_y)+" "+str(pos_z)+" "+str(ori_x)+" "+str(ori_y)+" "+str(ori_z)+" "+str(ori_w)+"\n")
    
    print("Save to file",timestamp)


if __name__ == '__main__':
    topic_name = sys.argv[1]
    out_path = sys.argv[2]
    subscriber_name = "subscriber_" + topic_name.split("/")[-1]

    fout = open(out_path,'a')

    print("waiting for message coming...")

    # 初始化节点
    rospy.init_node(subscriber_name)
    # 开始订阅
    rospy.Subscriber(topic_name, Odometry, callback)
    # 循环
    rospy.spin()

脚本需要输入两个参数,需要保存的Topic name,以及输出文件的路径,输出格式为TUM。相关代码传到了Github,点击查看。如下所示,就是保存轨迹的可视化,用的是这篇博客中介绍的evo工具。

5.参考资料

  • [1] http://wiki.ros.org/nav_msgs
  • [2] http://wiki.ros.org/geometry_msgs
  • [3] http://wiki.ros.org/sensor_msgs
  • [4] http://wiki.ros.org/stereo_msgs
  • [5] https://wiki.ros.org/std_msgs

本文作者原创,未经许可不得转载,谢谢配合

返回顶部