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中一类与几何相关的消息，包含常见的Point、Quaternion、Twist、Vector3、Pose等类型，其包含的全部类型具体列举如下。
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而言，这里面就有一些相对常用的类型，如Odometry和Path，具体包含如下类别：
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就是一系列和传感器有关的消息，一些常见的如Image、IMU、PointCloud类型都属于它。当然，因为传感器种类繁多，因此这里会有很多我们几乎从没见过的类型，但也列出来了，作为了解即可，需要的时候可供查阅。所包含的全部类型罗列如下。
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
本文作者原创,未经许可不得转载,谢谢配合
