Support ROS REP 147 Odometry Message

[jgoppert]

http://www.ros.org/reps/rep-0147.html

If we support this I think it will be very helpful. Currently we are using local_position_ned_cov + attitude_quaternion_cov to send the same info. We may still have to split the message into the attitude and position parts due to size, but we should at least add the frame_id and child_frame_id so that the messages can be used for various sensors (vision/mocap etc.) These frame ids could be implemented in mavlink using an enum to save space.

@tfoote
@LorenzMeier
@mhkabir
@TSC21

[TSC21]

Agree!

[jgoppert]

I currently am blocking on this for my ROS SLAM system, so I'm going to start adding it.

ROS side

# 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

Data Contained

• header

  • frame_id: frame that pose is expressed in, frame with which velocity is respect to
  • stamp: timestamp
  • seq: monotonic sequence number

• child_frame_id: frame which velocity vector is expressed in

• pose

  • pose: [x, y, z], [qw, qx, qy, qz] double
  • covariance: [6x6] double

• twist

  • twist: [vx, vy, vz], [p, q, r]
  • covariance: [6x6] double

• pose: [x, y, z, ], expressed in frame_id

MAVROS

• Support this odom msg in mavros_extras odometry plugin
• Add support for messages to pose publisher, publish to vision/mocap etc based on frame

Proposed MAVLink message

• frame enum

  • FRAME_MOCAP_NED [ origin for mocap]
  • FRAME_VISION_NED [ origin for vision ]
  • FRAME_ODOM_NED [ origin for odometry ]
  • FRAME_ESTIM_NED [ origin for estimate ]
  • FRAME_BODY_FRD (forward right down, fixed in body)
  • ? do we need any others, can have 256 in a 1 byte enum, but probably don't want to support them all on the firmware side

• examples:

  • these 2 byte frames will remove a lot of the duplication we have in various pose sensors that we current have (VISION_POSITION, VISION_VELOCITY, MOCAP_POSITOIN etc.)
  • to send mocap measurement to vehicle:
    • frame_id: FRAME_MOCAP_NED
    • child_frame_id: FRAME_BODY_FRD
  • to send vision measurement to vehicle:
    • frame_id: FRAME_VISION_NED
    • child_frame_id: FRAME_BODY_FRD
  • to receive estimate from vehicle:
    • frame_id: FRAME_ESTIM_NED
    • child_frame_id: FRAME_BODY_FRD

• message

  • frame_id [ enum, 1 byte]
  • child_frame_id [ enum, 1 byte]
  • pos [3 float, 12 bytes]
  • q [4 float, 16 bytes]
  • vel [3 float, 12 bytes]
  • ang_vel [3 float, 12 bytes]
  • pose covariance, upper triangle [21 float, 84 bytes] Cov([pos_err[0], pos_err[1], pos_err[2], roll_err[0], pitch_err[1], yaw_err[2])
  • twist covariance, upper triangle [21 float, 84 bytes] Cov([vel_err[0], vel_err[1], vel_err[2], ang_vel_err[0], ang_vel_err[1], ang_vel_err[2]])
  • total size: 222 bytes

• note, ROS odom doesn't support cross covariance between attitude and position. We can still handle this in IEKF and EKF2 and then export in the odom format. The higher level slam algorithms seem to work well enough assuming position and attitude states are independent.

[vooon]

Note float is 4-byte.

In [32]: struct.calcsize('<bb3f4f3f3f21f21f')
Out[32]: 222

Short float (16-bit) is not implemented by mavlink, but it is may be useful for covariances.

[jgoppert]

Ah, right.. oops, should still fit through, right, will be close. I believe 255 is the limit for the frame size and there is some overhead for check sum and header etc.

[vooon]

Yes, 255 is max payload length. Message a bit longer (header + payload + crc + optional signature).

[jgoppert]

OK, good. What do you think on field names, should we split twist and pose as I have done above (pos, vel, q, ang_vel), or just leave them as twist (length 6) and pose (length 7), I think either will work as long as we document it, but any preference?

[vooon]

I prefer vector = float[3] and quaternion = float[4]. But many of mavlink messages propose split of vectors to x,y,z fields.

[TSC21]

Even that the cross covariance between attitude and position is not supported in ROS, the proposed MAVLink odom msg may include a 6x6 covariance matrix that supports this.

[jgoppert]

That would cost us 6*6*4=144 bytes, so maybe in a separate message?

[TSC21]

That would cost us 664=144 bytes, so maybe in a separate message?

Yeah we can leave the upper-triangular like you propose and add a separate msg for full co-variances. But you can check that ROS has full 6x6 covariance matrices for pose and twist on its Odometry msg. But shouldn't the upper-triangular be equal to the lower in this case?

[jgoppert]

Yeah, the covariance matrix is always symmetric.

[TSC21]

Ok so as long we keep the symetry in the ROS side, I suppose we can send the URT of the cov matrix of the pose and of the twist in the MAVLink msg.

[LorenzMeier]

Could you send a PR with this? I'm closing meanwhile.

[TSC21]

@LorenzMeier on my todo list. I will issue a PR soon.