Frame conversions #317

TSC21 · 2015-06-26T17:57:05Z

Finally, after some tinkering with @BlackCoder, we were able to get to the proper conversions to position and orientation between ROS ENU and PX4 NED frames.
So, as this is a common code for many plugins, I decided to create a specific lib file where all the functions for frame conversions are installed. This functions are called by the plugins to do the conversions online.
ADD: This PR also updates some comments and doxygen documentation on plugins.

Right now, I think I'm just missing:

Add covariance frame conversion to uas_frame_conversions.cpp (partially)

Update: Relative to covariance conversions, I'm still missing:

Conversion to 7x7 full pose covariance matrix (attitude in quaternions instead of RPY)
Conversion to quaternion covariance only (based on the previous)
Note that transform_frame_covariance_pose6x6() can also be used to other covariances matrices, (p.e., lin_vel + RPY, ang_vel+RPY, and why not position+lin_vel since the conversion is the same - not very used)

Update2: I will leave the above covariances conversions to another PR.

This PR Fix #194, #148 (please continue list).

…correct frame conversion between ENU<->NED

tonybaltovski · 2015-06-26T18:41:21Z

mavros/src/lib/uas_frame_conversions.cpp

+	return tf::Vector3(x, y, z);
+};
+
+static tf::Vector3 convert_velocity(float _vx, float _vy, float _vz){


Why have different functions that perform the same conversion?

Just naming types

mhkabir · 2015-06-27T05:57:20Z

I suggest that the naming should be changed to "transform_frame" and no need to make 5x copies of the same function, since we have the exact same conversion for all data from NED to ENU and back.
@TSC21 @vooon @tonybaltovski

TSC21 · 2015-06-27T08:12:19Z

I suggest that the naming should be changed to "transform_frame"

Ok why not

5x copies of the same function

This was just a naming convention, as we also have functions to convert attitude (rpy and q). Like this we can name different functions for different physical variables (position, velocity, acceleration, ...). If not, how to call it? I created a general_xyz which is being used for any case that doesn't fit into the previous one.

…6 matrix

vooon · 2015-06-27T14:28:04Z

I don't see why you really need different function for each type. And if you use same conversion do one general func and call it in inlined pos, att etc. funcs.

Also as i see, there no more difference between body and local?
Conversion is same, but specialized ned-enu, enu-ned suffixes more readable, and describe what we want to do.

TSC21 · 2015-06-27T15:52:31Z

Ok i'll launch a general conversion function then. But as you say that there's no need to multiple functions to different types, i don't see why we have to say the direction since the conversion is exactly the same. The direction is marked on comments as you can see.

From flight tests done by @BlackCoder, it seems that there's no need to differentiate local from body frames. Mocap and vision plugins already flight tested. Just have to be sure that imu plugin data corresponds to what is expect (which it should since the rotation matrix values correspond to the previous transformations applied to pitch and yaw values). As to ENU to NED conversions used on plugins, it's all good to go, since it was tested and confirmed using a mocap system.

vooon · 2015-06-27T21:48:23Z

Best documentation is the code itself. Commenting each entry worse, than two inlined ned_enu, enu_ned variants.
Anyway compiller optimize to direct call underlying func.

vooon · 2015-06-27T21:49:33Z

And i see, that there difference in general and rpy. Why?

vooon · 2015-06-27T21:54:33Z

Also a note, that those functions are best target for unit tests. Now test exist only for libmavconn, because most plugins require real FCU (at least SITL), write test for them little complex.

TSC21 · 2015-06-27T23:16:13Z

Best documentation is the code itself. Commenting each entry worse, than two inlined ned_enu, enu_ned variants.

I still can't find a use to tell the direction on the function name, as the conversion is exactly the same.

vooon · 2015-06-28T09:49:02Z

Where did you find 7x7 covariance? geometry_msgs/PoseWithCovariance uses 6x6.

TSC21 · 2015-06-28T11:12:36Z

Where did you find 7x7 covariance? geometry_msgs/PoseWithCovariance uses 6x6.

Well 7x7 pose matrices exists and can be used. Check MRPT: http://www.mrpt.org/tutorials/programming/maths-and-geometry/2d_3d_geometry/#4_Conversions_between_classes.

Quaternion covariance also exists. Check page 7 of this paper.

So I assume a 7x7 covariance matrix can be built, even though ROS doesn't support it yet, but many applications may depend on a covariance matrix like this. The problem is that the propagation of covariance in quaternion representation is not so trivial.

…on; general doxygen comment cleanup

vooon · 2015-06-28T15:55:23Z

mavros/src/plugins/imu_pub.cpp

@@ -207,13 +206,12 @@ class IMUPubPlugin : public MavRosPlugin {

 		auto imu_msg = boost::make_shared<sensor_msgs::Imu>();

-		// PX4 NED (w x y z) -> ROS ENU (x -y -z w) (body-fixed)
-		tf::Quaternion orientation(att_q.q2, -att_q.q3, -att_q.q4, att_q.q1);
+		tf::Quaternion orientation(att_q.q1, att_q.q2, att_q.q3, att_q.q4);


NO! You missed that FCU and ROS use different field convention.
It still should be orientation(q2, q3, q4, q1).

FCU: w x y z
ROS: x y z w
right?

vooon · 2015-06-28T15:57:49Z

mavros/src/plugins/imu_pub.cpp

-			magn_msg->magnetic_field.x = imu_hr.ymag * GAUSS_TO_TESLA;
-			magn_msg->magnetic_field.y = imu_hr.xmag * GAUSS_TO_TESLA;
-			magn_msg->magnetic_field.z = -imu_hr.zmag * GAUSS_TO_TESLA;
+			auto mag_field = UAS::transform_frame_ned_enu_xyz(imu_hr.xmag, imu_hr.ymag, imu_hr.zmag);


There bee GAUSS units, not milliTESLA.

…ersion

Frame conversions

TSC21 force-pushed the frame_conversions branch from c134359 to 7c5a55a Compare June 26, 2015 17:58

frame_conversions: added frame_conversion specific lib file; applied …

bdf9715

…correct frame conversion between ENU<->NED

TSC21 force-pushed the frame_conversions branch from 7c5a55a to bdf9715 Compare June 26, 2015 18:03

tonybaltovski reviewed Jun 26, 2015
View reviewed changes

vooon added enhancement API change labels Jun 26, 2015

TSC21 force-pushed the frame_conversions branch 7 times, most recently from 6354923 to 8cd9fa7 Compare June 26, 2015 23:00

TSC21 force-pushed the frame_conversions branch from 8cd9fa7 to b02ff79 Compare June 27, 2015 08:07

frame_conversions: added covariance frame conversion for full pose 6x…

6cce473

…6 matrix

TSC21 force-pushed the frame_conversions branch from b02ff79 to 6cce473 Compare June 27, 2015 08:17

vooon added the testing required label Jun 27, 2015

TSC21 force-pushed the frame_conversions branch 5 times, most recently from 37d325e to 41f22ee Compare June 27, 2015 20:14

changed frame conversion func name; add 3x3 cov matrix frame conversi…

197fe70

…on; general doxygen comment cleanup

TSC21 force-pushed the frame_conversions branch from 3a143c9 to 197fe70 Compare June 28, 2015 11:20

This was referenced Jun 28, 2015

coordinate frame conversion on vision_position_estimate #225

Closed

Reiterate over frame conversions between NED and ENU #216

Closed

Generalize frame conversion routines as part of libmavros #194

Closed

TSC21 force-pushed the frame_conversions branch 7 times, most recently from fd511f0 to a22439d Compare June 28, 2015 14:51

vooon mentioned this pull request Jun 28, 2015

Release 0.12 #318

Closed

2 tasks

vooon added this to the Version 0.12 milestone Jun 28, 2015

TSC21 force-pushed the frame_conversions branch 3 times, most recently from 0cbb971 to ef12e98 Compare June 28, 2015 15:43

vooon reviewed Jun 28, 2015
View reviewed changes

TSC21 mentioned this pull request Jun 28, 2015

Conversion for covariances #148

Closed

vooon reviewed Jun 28, 2015
View reviewed changes

frame_conversions: use inline functions to identify direction of conv…

2f855b5

…ersion

TSC21 force-pushed the frame_conversions branch from ef12e98 to 2f855b5 Compare June 28, 2015 15:58

frame_conversion: last fix patch

266ab57

vooon added a commit that referenced this pull request Jun 28, 2015

Merge pull request #317 from TSC21/frame_conversions

e043a76

Frame conversions

vooon merged commit e043a76 into mavlink:master Jun 28, 2015

TSC21 deleted the frame_conversions branch June 28, 2015 16:29

SiddharthPatel45 mentioned this pull request Apr 14, 2017

Vision Estimates and/or setpoints not rotated properly (forwarded quaternions are wrong) #523

Closed

Timple mentioned this pull request May 14, 2020

fix signs of sensor_msgs::Imu output KumarRobotics/ublox#88

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Frame conversions #317

Frame conversions #317

TSC21 commented Jun 26, 2015

tonybaltovski Jun 26, 2015

TSC21 Jun 26, 2015

mhkabir commented Jun 27, 2015

TSC21 commented Jun 27, 2015

vooon commented Jun 27, 2015

TSC21 commented Jun 27, 2015

vooon commented Jun 27, 2015

vooon commented Jun 27, 2015

vooon commented Jun 27, 2015

TSC21 commented Jun 27, 2015

vooon commented Jun 28, 2015

TSC21 commented Jun 28, 2015

vooon Jun 28, 2015

TSC21 Jun 28, 2015

vooon Jun 28, 2015

vooon Jun 28, 2015

TSC21 Jun 28, 2015

Frame conversions #317

Frame conversions #317

Conversation

TSC21 commented Jun 26, 2015

tonybaltovski Jun 26, 2015

Choose a reason for hiding this comment

TSC21 Jun 26, 2015

Choose a reason for hiding this comment

mhkabir commented Jun 27, 2015

TSC21 commented Jun 27, 2015

vooon commented Jun 27, 2015

TSC21 commented Jun 27, 2015

vooon commented Jun 27, 2015

vooon commented Jun 27, 2015

vooon commented Jun 27, 2015

TSC21 commented Jun 27, 2015

vooon commented Jun 28, 2015

TSC21 commented Jun 28, 2015

vooon Jun 28, 2015

Choose a reason for hiding this comment

TSC21 Jun 28, 2015

Choose a reason for hiding this comment

vooon Jun 28, 2015

Choose a reason for hiding this comment

vooon Jun 28, 2015

Choose a reason for hiding this comment

TSC21 Jun 28, 2015

Choose a reason for hiding this comment