WIP: Add odometry message to local position plugin & coordinate frame review

[scott-eddy]

This PR will add a nav_msgs/Odometry output to the local position plugin, solving #470 directly. Additionally #438 will be addressed by this PR with an in-depth analysis of the current coordinate frames used, transformations between frames, and making these frames as REP 105 and REP 103 compliant with minimal impact on the current user base (e.g. with changing as few frame names as possible).

Coordinate Frame Examination

• Review frames currently in use
  • Determine if renaming frames to be REP 105 compliant is worth the disruption to current user base
  • Review rotations between current frames in uas_frame_conversions.cpp
  • Update frame naming and rotation methods as necessary

Odometry Message Publication

• Publish Odometry message
  • Determine proper frames to be placed in header and child frame within the message
  • Calculate linear and angular velocities in the associated child frame
  • Determine way in which to provide the pose and twist covariances from the local position message

Documentation

Here is a discussion of the proposed changes and why they are being made.

To help facilitate ensuring frames are properly rotated I've begun tabulating each plugin's data and what frame it is represented in on the FCU. Not being and Arducopter developer I will not be diving into frame representations on that firmware stack but have included it anyway if someone wants to take the lead on it.

[scott-eddy]

To address the first point of this I have created the following document.

@vooon, @LorenzMeier, @mhkabir. Input would be appreciated

[andre-nguyen]

https://github.com/mavlink/mavros/blob/master/mavros/src/lib/uas_frame_conversions.cpp#L24

Would it not be sufficient to just change

static const Eigen::Quaterniond FRAME_ROTATE_Q = UAS::quaternion_from_rpy(M_PI, 0.0, 0.0);

to

static const Eigen::Quaterniond FRAME_ROTATE_Q = UAS::quaternion_from_rpy(M_PI, 0.0, M_PI_2);

?

[scott-eddy]

@andre-nguyen this change would be sufficient for correcting the change from the ENU to NED frame. However, the reluctance to change this is because some messages (e.g. ones for IMU data) are actually expressed in the body frame not the NED frame as @mhkabir pointed out in #438.

If nothing else we need to have a body frame -> ENU frame and NED -> ENU frame conversion. But by making the FRAME_ROTATE_Q variable an enum we will be able to define an arbitrary number of useful rotations such as NED -> body frame or ENU -> custom frame. Also by making the transform depend on an enumeration if a specific transform was found to have a bug in it, then the entire API wouldn't need to be re-worked, just that specific rotations.

[andre-nguyen]

Good point

[scott-eddy]

One limitation I'm running into is that FRAME_ROTATE_Q by nature has to be a static rotation. This makes the NED -> body frame transformation not fit into the scope of the frame transformation lib as the quaternion from NED -> body, and for that matter ENU -> body, will always be changing.

I don't know how best to approach that piece, I think that the lib should remain a library of strict frame to frame definitions and that each plugin can convert things into the body frame as necessary.

[vooon]

It is ok for research, but later i think better to make two consts: FRAME_ROTATE_LOCAL_Q, FRAME_ROTATE_PLATFORM_Q.

Also platform? Not base?

[scott-eddy]

In light of the need of dynamic rotations (see discussion about @voon's 4th comment) I've implemented new functions for body->enu and body-> ned frames.

[vooon]

Check REP103, at least IMU should be reported in ros-body frame. And for that static rotation around Roll (X) is good enough.
Also real local-ned to local-enu same static transform. So only "ned-body" or "fcu-body" frame to "local-enu" require dynamic transform.

And here we may use TF, define several transforms:

Static:

• fcu_local → ros_local: ROLL +180° YAW +90°
• fcu_body → ros_body: ROLL +180°

Dynamic:

• fcu_body → ros_local

Then later anyone may check applied conversions in TF tree. But that approach cost in CPU usage than internal Eigen calculations.

[mhkabir]

@scott-eddy Looks to me that @vooon suggestion is good :)

[scott-eddy]

Sounds good to add the rotation from fcu_body to ros_body. Today I'll walk through and clean up all of the naming conventions and try to document it somewhere for clarity.

I'm still a little hazy on the desired implementation. Are we going to:

1. Use tf internally to perform all transformations.
2. Use Eigen internally for transforms, to save CPU, and only report transformations in the tf tree when setting tf_send to true?

2 would be more in line with how things are done now, and since CPU is generally limited on companion computer's I would vote we do it that way.

[vooon]

tf_send param not needed - static transform sent only once.

Perhaps 2-option is more effecient (since we do not need to resolve TF on each iteration), but if you can please add switch for static/tf selection.
I don't think that it is hard since we should use same code for body-local tf.

[scott-eddy]

@vooon see what you think about eac25cd. It includes a transform from aircraft (which is the fcu body-fixed frame) to base_link (which is the ros body-fixed frame). It also corrects the IMU publication.

[mhkabir]

With your latest push, what's the "aircraft" frame and the "base_link" frame referring to?

[scott-eddy]

@mhkabir base_link is the body-fixed frame expressed in ROS coordinates adhering to REP 105 and REP 103. The "aircraft" frame is the PX4 body-fixed frame.

I wanted to avoid calling one frame body and one base_link as both are rigidly attached to the vehicle. "aircraft" was chosen because the body-fixed frame for aircraft is usually defined with Z Down. Open to other naming conventions but it seems to make sense.

Obviously when all is said and done documentation needs to be developed, we should decide where to house it and how to inform other programs (e.g. PX4, Arducopter)

[scott-eddy]

Testing with 4a591db looks good. I ran some tests with the aircraft on the bench with the following:

• Start with aircraft on wheels (e.g. roll, pitch ==0)
• rotate to right wing down
• rotate to left wing down
• rotate to wheels
• rotate to nose down
• rotate to nose up.
• rotate to wheels

This yields the following acceleration profile:

While not intuitive, this is the correct rotation of the reported accelerations. I would have expected that when on wheels, one g would show up in the base_link frame as -9.8[m/s/s] as the g is aligned with the down direction. However, as explained to me in PX4/Firmware #3551 this is not the correct way to think about accelerometer measurements.

The bench test also provided the following angular rate profile:

These rates are correct for the base_link frame:

• positive x rate from wheels to right wing down, negative rate from right wing down to left wing, positive to return to wheels

• positive y rate from wheels to nose down, negative from nose down to nose up, positive to return to wheels

  @voon, @mhkabir do you agree that the readings are being reported correctly?

[mhkabir]

@scott-eddy Could you check the following accelerations quickly in the IMU data output (rostopic echo /mavros/imu/data):

• Measure +9.81 meters per second squared for the Z axis.
• If the FCU is rolled +90 degrees (left side up), the acceleration should be +9.81 meters per second squared for the Y axis.
• If the FCU is pitched +90 degrees (front side down), it should read -9.81 meters per second squared for the X axis.

[scott-eddy]

@mhkabir those are the accelerations measured with this. See above plot.

[scott-eddy]

Tested today that the orientation transformation from NED->aircraft to ENU->base_link is correct. After that I confirmed that rotating the reported NED velocities to the base_link frame results in accurate body-frame velocities. I shook the vehicle in the x axis while rotating it about the Z, then shook in the Y while rotating back the other way about the Z. I repeated this as follows:

• body X velocity
• body Y velocity
• body X velocity
• body Y velocity
• body X velocity
  The below plot confirms the rotations working as the body x and y velocities are nicely split.
  

[scott-eddy]

@vooon @mhkabir I'm confident the rotations are all working correctly now. How do we get this merged in? I can open a new PR with changes rebased to master. Also, I don't have a mocap facility to test some of the modules there.

What other testing needs to be performed?

[vooon]

Break of * "wall" looks ugly, but i may fix that later.

[vooon]

Also check please UAS::transform_frame_yaw().

[mhkabir]

That would be imu_pub for me, I think. Or maybe vision_pose.

Only those two are active for me.
On 29 Jan 2016 1:09 am, "Eddy Scott" notifications@github.com wrote:

@ALL https://github.com/all,help would be appreciated on this one. Its
not obvious to me where this is happening. What plugins are you running
when you have experience the crash? Can you try to nail it down to the one
where the problem is arising from?

—
Reply to this email directly or view it on GitHub
#473 (comment).

[scott-eddy]

@mhkabir do you get the error after running catkin clean --all and rebuilding. imu_pub doesn't produce errors on my end so if you can get it repeatably then it must be in vision_pose

[mhkabir]

@scott-eddy Yep, vision_pose. I'm listening to a TF from my VIO system, note, not the normal PoseStamped topic.

You might want to check that code path.

[scott-eddy]

@mhkabir I'll look into this in more depth sometime soon, will ping you when I think I have found something.

@voon, I will address the comments you made above.

[scott-eddy]

@mhkabir The only place that I can see an Eigen::Quaternion within vision pose is here. However, the function is transform_orientation which should be okay.

@tonybaltovski doesn't Eigen::Quaternion<double, 0> define a quaternion with dimension = 0? Perhaps that contributes to the problem.

[vooon]

CI failed. ROS_BREAK();, not ROS_BREAK(false);.
Also try to add #include <ros/assert.h>

[scott-eddy]

Fixed ROS_BREAK, did not have to #include <ros/asser.h>

[vooon]

@scott-eddy @mhkabir seems most problems are fixed, so what we need is testing?

[vooon]

TODO: update README.

[scott-eddy]

@vooon I still have not tracked down the problem that @mhkabir is having. I think that is the lingering issue

[scott-eddy]

@mhkabir I tested this with vision_pose running, sending a geometry_msgs/PoseStamped from rqt at a rate of 10Hz with no problem. I can't seem to duplicate the bug you have seen

[scott-eddy]

@mhkabir can you send me a gist of your config and pluginlist files

[scott-eddy]

@vooon I just spoke with @mhkabir, he was able to trace his problem to old build files on his system. We've both been able to run this with no issues. It is good to go.

[AlexisTM]

On mavros 0.18.4 and PX4 1.5.1.

I am sending a "almost null yaw" (as I am at 0° rotation) quaternion : (x:0, y:0, z:0, w:1) to /mavros/vision_pose/pose (with external heading set to vision) and the resulting quaternion in /mavros/local_position/pose is almost 180° (x:0, y:0, z:-1, w:0)