[WIP] Set framework to define offset between global origin and current local position #691

TSC21 · 2017-04-06T20:18:17Z

This is WIP and pretends to implement a framework to set the global origin of the system so it can be used to compute the offset between the current local_position_ned of the drone and this same global origin. Useful for flexible nav between GPS denied environments and GPS permitted environments.

Add wrappers for GPS_GLOBAL_ORIGIN and SET_GPS_GLOBAL_ORIGIN
Add handler for LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET

TSC21 · 2017-04-06T20:21:17Z

Somehow latest https://github.com/ros-geographic-info/geographic_info available on release package doesn't include http://docs.ros.org/api/geographic_msgs/html/msg/GeoPointStamped.html. Will need to wait for ros-geographic-info/geographic_info@b15dc33 to be included - at the same time, also mavlink/mavlink#696 so to be able to use the time-stamp.

TSC21 · 2017-04-06T20:38:10Z

Travis failing because of undated geographic_msgs package on release.

vooon · 2017-04-07T08:04:39Z

Where translation of int32 encoded lat/long to double and back?

TSC21 · 2017-04-07T11:50:53Z

Where translation of int32 encoded lat/long to double and back?

Fixed in c4941a9

vooon · 2017-04-07T17:10:51Z

mavros/src/plugins/global_position.cpp

-		g_origin->position.altitude = glob_orig.altitude;
+		g_origin->position.latitude = (double)glob_orig.latitude; // @warning TODO: #529
+		g_origin->position.longitude = (double)glob_orig.longitude;
+		g_origin->position.altitude = (double)glob_orig.altitude;


Read message documentation! That is not how you should convert int32 encoded coord's to floating point.

TSC21 · 2017-04-07T18:17:21Z

We still need to consider #529

TSC21 · 2017-04-09T18:25:35Z

@mhkabir from your perspective, the LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET should be computed onboard or offboard? From the FCU perspective only, probably onboard, but if you are using a companion board, I feel it does the same.

TSC21 · 2017-04-15T14:49:53Z

Still waiting fo rosbuild farm update. I was expecting that geographic_msgs was already updated. Also, after #693 is merged, I will update this with the geoid<->ellipsoid height calculation.

TSC21 · 2017-05-05T08:20:31Z

Seems there's still no new release for indigo so travis is still failing when looking for new msg. In the other hand, the releases for jade and kinetic are already out.

TSC21 · 2017-05-15T20:23:02Z

Great. So now we have this ready for indigo also. Will push the handler for LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, fix the code style.

…l_origin_offset

vooon · 2017-05-16T16:05:13Z

mavros/src/plugins/global_position.cpp

+			gps_cov(2, 0) , gps_cov(2, 1) , gps_cov(2, 2) , 0.0     , 0.0     , 0.0     ,
+			0.0           , 0.0           , 0.0           , rot_cov , 0.0     , 0.0     ,
+			0.0           , 0.0           , 0.0           , 0.0     , rot_cov , 0.0     ,
+			0.0           , 0.0           , 0.0           , 0.0     , 0.0     , rot_cov ;


Hmm, we probably may do that:

pos_cov_out.setZero(); pos_cov_out.block<3, 3>(0, 0) = gps_cov; pos_cov_out.block<3, 3>(3, 3).diagonal() = rot_cov;

yeah. more reasonable

TSC21 · 2017-05-16T16:33:18Z

This seems reasonable to me now.

At mavlink side we can probably add:

SET_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET msg
MAV_CMD_SET_GPS_GLOBAL_ORIGIN cmd

TSC21 · 2017-05-27T17:54:58Z

Ok according to REP 105, if we have a case where we may have different map frames (that may be associated to the same mobile platform or possibly to different platforms) we should define a global reference that is fixed even when different map frames are created (as a consequence of, for example, have transitions from global to local frames, resulting on having different odom and map frames).

This same coordinate frame should be called earth and, in our case, would be useful for defining the global origin of the system. It would also be useful for multiple systems interactions.

This same coordinate system should be represented in the ECEF ("earth-centered, earth-fixed") geographic coordinate system, so this involves a conversion from AMSL to ECEF, that is supported by geographiclib. It can be added after #693 is merged.

@jgoppert thanks for pointing this out.

We can expect also that geographic_msgs to be updated to support this same convention (ros-geographic-info/geographic_info#18).

…s84, frame_id!=global_frame_id)

vooon

Did you tested?

vooon · 2017-06-06T20:29:04Z

mavros/src/plugins/global_position.cpp

@@ -70,14 +71,24 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 		gp_odom_pub = gp_nh.advertise<nav_msgs::Odometry>("local", 10);
 		gp_rel_alt_pub = gp_nh.advertise<std_msgs::Float64>("rel_alt", 10);
 		gp_hdg_pub = gp_nh.advertise<std_msgs::Float64>("compass_hdg", 10);
+
+		// global origin
+		gp_global_origin_pub = gp_nh.advertise<geographic_msgs::GeoPointStamped>("gp_origin", 10);


Why not latching type?

what do you mean with latching type?

vooon · 2017-06-06T20:31:00Z

mavros/src/plugins/global_position.cpp

-				make_handler(&GlobalPositionPlugin::handle_global_position_int)
+			       make_handler(&GlobalPositionPlugin::handle_global_position_int),
+			       make_handler(&GlobalPositionPlugin::handle_gps_global_origin),
+			       make_handler(&GlobalPositionPlugin::handle_lpned_system_global_offset)


Wtf here with tab/space?

can't tell. uncrustify made this

vooon · 2017-06-06T20:33:50Z

mavros/src/plugins/global_position.cpp

+		msg.latitude = point->position.latitude * 1E7;		// deg
+		msg.longitude = point->position.longitude * 1E7;	// deg
+		msg.altitude = point->position.altitude * 1E3;		// m
+	}


We need more mess with fill_xyz()!

vooon · 2017-06-06T20:36:24Z

mavros/src/plugins/global_position.cpp

@@ -127,7 +160,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 			fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
 		}

-		fill_lla(raw_gps, fix);
+		fill_lla_wgs84(raw_gps, fix);


And altitude is still AMSL.

yes cause we are missing #693. Or we first merge this or the other

vooon · 2017-06-06T20:37:29Z

mavros/src/plugins/global_position.cpp

+
+		g_origin->header.frame_id = tf_global_frame_id;
+		g_origin->header.stamp = ros::Time::now();
+		fill_lla_ecef(glob_orig, g_origin);	// @warning TODO: #693


ok I can push it to the body. I just then have to sync with #693 then.

vooon · 2017-06-06T20:41:26Z

mavros/src/plugins/global_position.cpp

-			0.0           , 0.0           , 0.0           , 0.0     , 0.0     , rot_cov ;
+		pos_cov_out.setZero();
+		pos_cov_out.block<3, 3>(0, 0) << gps_cov;
+		pos_cov_out.block<3, 3>(3, 3).diagonal() << rot_cov;


I'm not sure, but odom should be zeroed at make_shared(). Also use operator=, and check that scalar rot_cov actually assigned to whole vector.

covariance: [0.656100003862381, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.656100003862381, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.624400015449524, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]

for:

pos_cov_out.setZero(); pos_cov_out.block<3, 3>(0, 0) = gps_cov; pos_cov_out.block<3, 3>(3, 3).diagonal() << rot_cov, rot_cov, rot_cov;

vooon · 2017-06-06T20:42:21Z

mavros/src/plugins/global_position.cpp

+
+		gpo.target_system = m_uas->get_tgt_system();
+		// gpo.time_boot_ms = stamp.toNSec() / 1000;	#TODO: requires Mavlink msg update
+		fill_lla_amsl(req, gpo);// @warning TODO: #693


Also used once.

TSC21 · 2017-06-06T21:01:15Z

Did you tested?

This requires cross-test with #693. So please review #693 so we merge it first and then I can validate this one.

…t local position (mavlink#691) * add handlers for GPS_GLOBAL_ORIGIN and SET_GPS_GLOBAL_ORIGIN * fix cast of encoding types * refactor gps coord conversions * uncrustify * global_position: add LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET handler * global_position: add trasform sender for offset * global_origin: refactor covariance matrix * global_position: update copyright * global_position: add initial support to REP 105 * px4_config: global_position: update frame description * global_position: correct identation * global position: be consistent with frame and methods names (ecef!=wgs84, frame_id!=global_frame_id) * global_position: updates to code structure * global_position: fix identation

add handlers for GPS_GLOBAL_ORIGIN and SET_GPS_GLOBAL_ORIGIN

92b275c

TSC21 self-assigned this Apr 6, 2017

TSC21 added PX4 plugin enhancement labels Apr 6, 2017

fix cast of encoding types

c4941a9

vooon reviewed Apr 7, 2017

View reviewed changes

refactor gps coord conversions

5828670

uncrustify

5ec3c56

TSC21 closed this Apr 9, 2017

TSC21 reopened this Apr 9, 2017

TSC21 closed this Apr 14, 2017

TSC21 reopened this Apr 14, 2017

TSC21 closed this Apr 15, 2017

TSC21 reopened this Apr 15, 2017

TSC21 closed this Apr 17, 2017

TSC21 reopened this Apr 17, 2017

TSC21 closed this May 5, 2017

TSC21 reopened this May 5, 2017

TSC21 closed this May 10, 2017

TSC21 reopened this May 10, 2017

TSC21 closed this May 11, 2017

TSC21 reopened this May 11, 2017

TSC21 closed this May 15, 2017

TSC21 reopened this May 15, 2017

TSC21 added 3 commits May 16, 2017 17:21

Merge branch 'master' of https://github.com/mavlink/mavros into globa…

55315c4

…l_origin_offset

global_position: add LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET handler

b732739

global_position: add trasform sender for offset

64111d4

vooon reviewed May 16, 2017

View reviewed changes

global_origin: refactor covariance matrix

8215c7c

global_position: update copyright

cbf87be

TSC21 mentioned this pull request May 27, 2017

geodesy: add support for conversions to and from ECEF ros-geographic-info/geographic_info#18

Open

TSC21 added 3 commits May 27, 2017 20:39

global_position: add initial support to REP 105

9420462

px4_config: global_position: update frame description

35b0817

global_position: correct identation

555c3f1

TSC21 force-pushed the global_origin_offset branch from e5c6ef2 to 555c3f1 Compare May 27, 2017 17:44

TSC21 mentioned this pull request Jun 6, 2017

Use GeographicLib tools to guarantee ROS msg def and enhance features #693

Merged

3 tasks

global position: be consistent with frame and methods names (ecef!=wg…

855835e

…s84, frame_id!=global_frame_id)

vooon requested changes Jun 6, 2017

View reviewed changes

vooon added the testing required label Jun 6, 2017

TSC21 added 2 commits June 6, 2017 22:25

global_position: updates to code structure

fd1fe5e

global_position: fix identation

4f4127a

vooon approved these changes Jun 7, 2017

View reviewed changes

vooon merged commit 864115a into mavlink:master Jun 7, 2017

vooon added this to the Version 0.20 milestone Jun 7, 2017

TSC21 deleted the global_origin_offset branch June 7, 2017 08:22

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[WIP] Set framework to define offset between global origin and current local position #691

[WIP] Set framework to define offset between global origin and current local position #691

TSC21 commented Apr 6, 2017 •

edited

Loading

TSC21 commented Apr 6, 2017 •

edited

Loading

TSC21 commented Apr 6, 2017

vooon commented Apr 7, 2017

TSC21 commented Apr 7, 2017

vooon Apr 7, 2017

TSC21 commented Apr 7, 2017

TSC21 commented Apr 9, 2017

TSC21 commented Apr 15, 2017 •

edited

Loading

TSC21 commented May 5, 2017 •

edited

Loading

TSC21 commented May 15, 2017

vooon May 16, 2017

TSC21 May 16, 2017

TSC21 commented May 16, 2017

TSC21 commented May 27, 2017 •

edited

Loading

vooon left a comment

vooon Jun 6, 2017

TSC21 Jun 6, 2017

vooon Jun 6, 2017

TSC21 Jun 6, 2017

vooon Jun 6, 2017

TSC21 Jun 6, 2017

vooon Jun 6, 2017

TSC21 Jun 6, 2017

vooon Jun 6, 2017

TSC21 Jun 6, 2017

vooon Jun 6, 2017

TSC21 Jun 6, 2017

TSC21 Jun 6, 2017 •

edited

Loading

vooon Jun 6, 2017

TSC21 commented Jun 6, 2017

[WIP] Set framework to define offset between global origin and current local position #691

[WIP] Set framework to define offset between global origin and current local position #691

Conversation

TSC21 commented Apr 6, 2017 • edited Loading

TSC21 commented Apr 6, 2017 • edited Loading

TSC21 commented Apr 6, 2017

vooon commented Apr 7, 2017

TSC21 commented Apr 7, 2017

Choose a reason for hiding this comment

TSC21 commented Apr 7, 2017

TSC21 commented Apr 9, 2017

TSC21 commented Apr 15, 2017 • edited Loading

TSC21 commented May 5, 2017 • edited Loading

TSC21 commented May 15, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented May 16, 2017

TSC21 commented May 27, 2017 • edited Loading

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

Choose a reason for hiding this comment

TSC21 Jun 6, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented Jun 6, 2017

TSC21 commented Apr 6, 2017 •

edited

Loading

TSC21 commented Apr 6, 2017 •

edited

Loading

TSC21 commented Apr 15, 2017 •

edited

Loading

TSC21 commented May 5, 2017 •

edited

Loading

TSC21 commented May 27, 2017 •

edited

Loading

TSC21 Jun 6, 2017 •

edited

Loading