WIP: Add odometry plugin. #706

jgoppert · 2017-05-06T21:41:21Z

This adds an odometry plugin that simplifies interaction with ROS based estimators.

TSC21

I think it is simple and sharp. we just need to clean debug msgs and add co-variance frame tf

TSC21 · 2017-05-06T21:42:50Z

mavros_extras/src/plugins/odom.cpp

+
+	void odom_cb(const nav_msgs::Odometry::ConstPtr &odom)
+	{
+		//ROS_INFO("odometry callback");


maybe we can clear this?

TSC21 · 2017-05-06T21:43:37Z

mavros_extras/src/plugins/odom.cpp

+		size_t i = 0;
+		for (int row=0; row < 6; row++) {
+			for (int col=row; col < 6; col++) {
+				//ROS_INFO("row: %d, col:%d, i:%d", row, col, i);


also this...

TSC21 · 2017-05-06T21:44:15Z

mavros_extras/src/plugins/odom.cpp

+				i += 1;
+			}
+		}
+		UAS_FCU(m_uas)->send_message_ignore_drop(lpos);


maybe you can put both msg send on the end?

I think that not needed. Fill one message + send is fine.

TSC21 · 2017-05-06T21:44:52Z

mavros_extras/src/plugins/odom.cpp

+		for (int row=0; row < 6; row++) {
+			for (int col=row; col < 6; col++) {
+				//ROS_INFO("row: %d, col:%d, i:%d", row, col, i);
+				lpos.covariance[i] = odom->pose.covariance[row*6 + col];


we need to apply frame transform from ENU to NED. Maybe we can discuss this and add it to frame_tf.h

I think better to finish ftf::transform_frame(Covariance6d) function implementation and use it.

yeah @vooon that's the intention

TSC21 · 2017-05-06T21:48:19Z

mavros_extras/src/plugins/odom.cpp

+		lpos.ay = 0;
+		lpos.az = 0;
+		size_t i = 0;
+		for (int row=0; row < 6; row++) {


probably better to create a std::array and use std::copy

TSC21 · 2017-05-06T21:48:49Z

mavros_extras/src/plugins/odom.cpp

+		att.q[1] = odom->pose.pose.orientation.x;
+		att.q[2] = odom->pose.pose.orientation.y;
+		att.q[3] = odom->pose.pose.orientation.z;
+		for (size_t i=0; i < 9; i++) {


std::copy suits better I think

vooon · 2017-05-07T08:05:46Z

mavros_extras/src/plugins/odom.cpp

+		lpos.z = -odom->pose.pose.position.z;
+		lpos.vx = odom->twist.twist.linear.y;
+		lpos.vy = odom->twist.twist.linear.x;
+		lpos.vz = -odom->twist.twist.linear.z;


Use tft::transform_frame_enu_ned() from frame_tf.h

Also i'm more like to use cog.py to generate msg-to-msg copy code.

yeah forgot that transform also

vooon · 2017-05-07T08:09:05Z

mavros_extras/src/plugins/odom.cpp

+		att.q[0] = odom->pose.pose.orientation.w;
+		att.q[1] = odom->pose.pose.orientation.x;
+		att.q[2] = odom->pose.pose.orientation.y;
+		att.q[3] = odom->pose.pose.orientation.z;


ftf::quaternion_to_mavlink().

[WIP] Odometry Plugin review

vooon · 2017-05-09T17:55:31Z

mavros_extras/src/plugins/odom.cpp

+		// for f in "xyz":
+		//     cog.outl("lpos.v%s = lin_vel_ned.%s();" % (f, f))
+		// for f in "xyz":
+		//     cog.outl("lpos.a%s = 0.0;" % f)


Why not:

for a, b in (('', 'pos_ned'), ('v', 'lin_vel_ned'), ('a', 'zero')): for f in 'xyz': cog.outl("lpos.{a}{f} = {b}.{f}();".format(**locals()))

*add

const auto zero = Eigen::Vector3d::Zero();

yeah makes it simples. thanks

vooon · 2017-05-09T18:05:57Z

mavros_extras/src/plugins/odom.cpp

+		// TODO: apply ftf::transform_frame(Covariance9d)
+		for (size_t i = 0; i < 9; i++) {
+			att.covariance[i] = odom->pose.covariance[i];
+		}


I assume that std::copy() shall correctly move array<double, 9> to array<float, 9>, because it uses iterators.

std::copy(odom->pose.covariance.cbegin(), odom->pose.covariance.cend(), att.covariance.begin());

Odom review part 2

vooon

Now looks better. One thing that i'm not sure: maybe call it send_odom?

vooon · 2017-05-10T05:48:53Z

mavros/src/lib/ftf_frame_conversions.cpp

+	case StaticTF::AIRCRAFT_TO_BASELINK:
+	case StaticTF::BASELINK_TO_AIRCRAFT:
+		Affine6dTf << AIRCRAFT_BASELINK_AFFINE * R, Eigen::MatrixXd::Zero(3, 3),
+		Eigen::MatrixXd::Zero(3, 3), AIRCRAFT_BASELINK_AFFINE * R;


Uncrustify often breaks indent in eigen matrixes. I think Second line should have identical indent as after <<.

vooon · 2017-05-10T05:49:57Z

mavros_extras/src/plugins/odom.cpp

-		// tf params
-		_odom_sub = _nh.subscribe("odom", 10, &OdometryPlugin::odom_cb, this);
+		// general params
+		odom_nh.param("estimator_type", estimator_type, 3); // defaulted to VIO type


using mavlink::common::MAV_ESTIMATOR_TYPE; // and then enum_value(MAV_ESTIMATOR_TYPE::VIO);

I kept it as int so it can be easily received and modified on the yaml file.

You can receive int from rosparam, but i think better to store it in it's enum class. But i may be wrong, since we will static_cast several times.

We should give the option to set it on rasparam IMO.

vooon · 2017-05-10T05:50:21Z

mavros_extras/src/plugins/odom.cpp

 	ros::Subscriber _odom_sub;

+	int estimator_type;


Why int and not MAV_ESTIMATOR_TYPE?

b08ffc0#r115705809

vooon · 2017-05-10T05:51:29Z

mavros_extras/src/plugins/odom.cpp

@@ -49,14 +51,15 @@ class OdometryPlugin : public plugin::PluginBase {
 	}

 private:
-	ros::NodeHandle _nh;
+	ros::NodeHandle odom_nh;
 	ros::Subscriber _odom_sub;


nh is fine. I don't like starting _, prefer m_ instead. But that isn't a problem.

vooon · 2017-05-10T05:52:43Z

mavros_extras/src/plugins/odom.cpp


 		lpos.time_usec = stamp;
+		lpos.estimator_type = estimator_type;
+		ROS_DEBUG_STREAM_NAMED("odom", "Odometry: estimator type: " << utils::to_string_enum<mavlink::common::MAV_ESTIMATOR_TYPE>(estimator_type));


Why here? I think put that into initialize() is enough.

vooon · 2017-05-10T05:53:48Z

mavros_extras/src/plugins/odom.cpp

+				i += 1;
+			}
+		}
+		UAS_FCU(m_uas)->send_message_ignore_drop(lpos);


I think that not needed. Fill one message + send is fine.

vooon · 2017-05-10T05:55:11Z

mavros/include/mavros/frame_tf.h

+	for (size_t i = 0; i < SIZE; i++) {
+		covmsg[i] = cov[i];
+	}
+}


Why? std::copy() does not work?

TSC21 · 2017-05-10T08:34:38Z

Now looks better. One thing that i'm not sure: maybe call it send_odom?

As it is I think we could but probably the next set should be add handlers for LOCAL_POSITION_NED_COV and ATTITUDE_QUATERNION_COV and publish those in a ROS Odometry msg.

TSC21 · 2017-05-10T10:34:30Z

Ok right now there are two things left to be fixed:

Understand why is the code giving matrix out of bounds for the covariance
Understand how to properly apply the rotation for angular velocity to the body frame

@jgoppert will do some tests on the simulation as soon as I can.

TSC21 · 2017-05-14T20:13:00Z

mavros/src/lib/ftf_frame_conversions.cpp

+ * @brief Auxiliar matrices to Covariance transforms
+ */
+using Affine6dTF = Eigen::Matrix<double, 6, 6>;
+using Affine9dTF = Eigen::Matrix<double, 9, 9>;


I probably have to, instead of creating a affine 6d transform (which seems not being working correctly), apply a Affine3d for each rotation and then build the covariance matrix with the diagonal blocks already rotated - the current output seems that it is just multiplying the matrices (rotation times covariance) but not applying the rotation the main diagonal blocks (which is expected from affine times matrix).

Here's an example:

[ INFO] [1494793156.340441773, 8.962000000]: Odometry: Cov URT: 4.97062e-10 -1.50062e-57 0 0 0 0 0 0 0 0 4.97062e-10 -1.26289e-42 0 0 0 0 0 0 0 0 4.97062e-10 0 0 0 0 0 0 0 0 0 0.0002 -6.03797e-52 0 0 0 0 0 0 0 0 0.0002 -5.08144e-37 0 0 0 0 0 0 0 0 0.0002 0 0 0 0 0 0 0 0 0 1 -3.01899e-48 0 0 0 0 0 0 0 0 1 -2.54072e-33 0 0 0 0 0 0 0 0 1

mhkabir · 2017-05-14T20:31:48Z

Hey guys, I can help out with this now that I have some free time. It would be good to get a brief on the open stuff @vooon @TSC21. Also squashing the commits would be nice - the diff looks very clean already.

I did a plugin (not upstreamable) when I developed the API for PX4 (PX4/PX4-Autopilot#6074). You can find that here : https://github.com/ProjectArtemis/mavros/blob/f043fa951657aa74c0219da836329367a395208f/mavros_extras/src/plugins/estimator_bridge.cpp

It does a bunch of things, and the frame transforms are all verified, and it has several hours of GPS-denied flight on it. We have a stable version of that interface running on some of our vehicles out in the field in active service.

TSC21 · 2017-05-14T20:47:22Z

Thanks for the help @mhkabir. But taking a look at your code seems that you are hardcoding the covariance. The purpose right now is apply the transform over a covariance being received from an external estimator. I already know what to do, so tomorrow I will push the final change so to merge this. Cheers

TSC21 · 2017-05-15T19:11:43Z

The new URT of the rotated covariance after the new commit:

 1.44017e-27  6.48596e-12  7.94301e-28            0            0            0            0            0            0
           0 -1.44017e-27 -1.59891e-43            0            0            0            0            0            0
           0            0 -6.48596e-12            0            0            0            0            0            0
           0            0            0  4.44089e-20       0.0002  2.44929e-20            0            0            0
           0            0            0            0 -4.44089e-20 -4.93038e-36            0            0            0
           0            0            0            0            0      -0.0002            0            0            0
           0            0            0            0            0            0  2.22045e-16            1  1.22465e-16
           0            0            0            0            0            0            0 -2.22045e-16 -2.46519e-32
           0            0            0            0            0            0            0            0           -1

Seems an acceptable rotation now.
So it lgtm in my opinion.
@jgoppert, @vooon should we merge this then?

TSC21 · 2017-05-16T14:10:40Z

After talking with @jgoppert I came to the conclusion that the first implementation was right. So I remove the last changes and apply a definitive commit.
Here's the latest covariance result after rotation:

[ INFO] [1494943715.279968291, 11.882000000]: Odometry: Cov URT: 
 2.20784e-10 -6.66544e-58            0            0            0            0            0            0            0
           0  2.20784e-10  -5.6095e-43            0            0            0            0            0            0
           0            0  2.20784e-10            0            0            0            0            0            0
           0            0            0       0.0002 -6.03797e-52            0            0            0            0
           0            0            0            0       0.0002 -5.08144e-37            0            0            0
           0            0            0            0            0       0.0002            0            0            0
           0            0            0            0            0            0            1 -3.01899e-48            0
           0            0            0            0            0            0            0            1 -2.54072e-33
           0            0            0            0            0            0            0            0            1

TSC21 · 2017-05-16T14:19:10Z

In any case we should consider, in the future, rely on tf2 to apply the transforms.

@jgoppert, @vooon lgtm?

jgoppert · 2017-05-16T14:27:38Z

Yeah, that diagonal looks good now.

jgoppert · 2017-05-16T14:30:11Z

mavros_extras/src/plugins/odom.cpp

+/**
+ * @brief Odometry plugin
+ * @file odom.cpp
+ * @author James Goppert <james.goppert8@gmail.com>


@TSC21 should probably add your name here, and also can you fix my email above, has an extra 8.

Mail fixed. No need to add my email. It was your idea and your implementation. I just added some features and robustness ;)

vooon · 2017-05-16T16:45:47Z

Oh, NOOOO! :) I mean in global_position, where huge matrix looks not great.
But in tf that small

r << m, 0, 0,
     0, m, 0,
     0, 0, m;

Way better than:

r.block(0,0) = m;
r.block(1,1) = m;
r.block(2,2) = m;

TSC21 · 2017-05-16T16:53:02Z

We have 6d and 9d matrices. What do you mean by small?
This avoids creating extra eigen matrices and just use blocks.

vooon · 2017-05-16T17:08:44Z

I mean that m visually hide 9d assign to looks like 3d.
But yes, assigning of blocks should be faster. And in some cases m.setZero() is not needed.

TSC21 · 2017-05-16T18:42:34Z

@vooon lgtm?

mhkabir · 2017-05-16T19:16:59Z

Nuno, lgtm is "Looks Good To Me" :D

TSC21 · 2017-05-16T19:24:17Z

Nuno, lgtm is "Looks Good To Me" :D

Always learning xD well I was asking looks good to Merge eheh

TSC21 · 2017-05-16T19:26:44Z

Can we merge this then?

vooon · 2017-05-17T06:55:44Z

Yes, LGTM. But i will manually rebase it to master.

vooon · 2017-05-17T07:15:02Z

@jgoppert merge of @TSC21 changes broke rebase. @TSC21 are you ok if i squash all into single commit?

TSC21 · 2017-05-17T07:16:45Z

Sure no prob @vooon

vooon · 2017-05-17T07:31:49Z

I will try to apply full patch and then edit history to split lib frame_tf changes and plugin. But it not succeed then i simply squash & commit.

vooon · 2017-05-17T11:41:59Z

Rebased to master. @jgoppert, @TSC21 please test because i done some style changes that may result in different math results (but should not!).

Add odometry plugin.

74bd6b2

TSC21 requested changes May 6, 2017

View reviewed changes

vooon requested changes May 7, 2017

View reviewed changes

jgoppert mentioned this pull request May 8, 2017

Support for absolute position/ attitude measurements introlab/rtabmap#188

Open

TSC21 and others added 3 commits May 9, 2017 14:34

extras: odom: use transform functions

bb8ad70

extras: odom: apply cog

53be8c8

Merge pull request #707 from TSC21/odom_review

2bf8e92

[WIP] Odometry Plugin review

vooon reviewed May 9, 2017

View reviewed changes

TSC21 and others added 6 commits May 9, 2017 21:22

lib: frame_tf: add covariance frame transforms

6ebfde0

lib: enum_to_string: add MAV_ESTIMATOR_TYPE enum_to_string

bbd7067

lib: remove duplicated type

c4763cc

odom: wip: try to apply attitude dependent transform

7a6bcbf

Merge pull request #708 from TSC21/cov_rotations

b08ffc0

Odom review part 2

Disable failing covariance.

aceb751

vooon requested changes May 10, 2017

View reviewed changes

lib: frame_conversions: fix Eigen matrix ident

b9d77c5

TSC21 added 6 commits May 10, 2017 11:40

extras: odom: minor fixes

5f2e04f

Merge branch 'odom' of https://github.com/mavlink/mavros into odom

cffe03c

frame_tf: covariance: use std::copy

d019136

extras: odom: keep parameter as int

18c15d0

extras: odom: improve cog code

1063b5b

lib: frame conversions: change matrix ident

9690663

TSC21 assigned jgoppert May 10, 2017

TSC21 added plugin extras labels May 10, 2017

TSC21 modified the milestone: Version 0.20 May 10, 2017

TSC21 reviewed May 14, 2017

View reviewed changes

frame_tf: use boost::array instead of std:array

829d4f7

lib: frame conversions: fix ident

3fb05be

TSC21 force-pushed the odom branch 2 times, most recently from 93f2292 to e76d1b2 Compare May 15, 2017 20:21

extras: odom: crystalize

98b57a5

TSC21 force-pushed the odom branch from e76d1b2 to 98b57a5 Compare May 16, 2017 14:18

jgoppert commented May 16, 2017

View reviewed changes

TSC21 added 2 commits May 16, 2017 17:39

extras: odom: fix e-mail

fe80b7f

extras/lib: improve covariance matrices code style

8b8e6c4

vooon mentioned this pull request May 17, 2017

Extras: Odometry plugin #716

Merged

vooon closed this May 17, 2017

WIP: Add odometry plugin. #706

WIP: Add odometry plugin. #706

Conversation

jgoppert commented May 6, 2017

TSC21 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

vooon left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented May 10, 2017

TSC21 commented May 10, 2017

TSC21 May 14, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mhkabir commented May 14, 2017

TSC21 commented May 14, 2017

TSC21 commented May 15, 2017 • edited Loading

TSC21 commented May 16, 2017

TSC21 commented May 16, 2017

jgoppert commented May 16, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

vooon commented May 16, 2017

TSC21 commented May 16, 2017 • edited Loading

vooon commented May 16, 2017

TSC21 commented May 16, 2017

mhkabir commented May 16, 2017

TSC21 commented May 16, 2017

TSC21 commented May 16, 2017

vooon commented May 17, 2017

vooon commented May 17, 2017

TSC21 commented May 17, 2017

vooon commented May 17, 2017

vooon commented May 17, 2017

TSC21 May 14, 2017 •

edited

Loading

TSC21 commented May 15, 2017 •

edited

Loading

TSC21 commented May 16, 2017 •

edited

Loading