Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Merged
merged 15 commits into from
Jun 7, 2017

Conversation

TSC21
Copy link
Member

@TSC21 TSC21 commented Apr 6, 2017

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
Copy link
Member Author

TSC21 commented Apr 6, 2017

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
Copy link
Member Author

TSC21 commented Apr 6, 2017

Travis failing because of undated geographic_msgs package on release.

@vooon
Copy link
Member

vooon commented Apr 7, 2017

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

@TSC21
Copy link
Member Author

TSC21 commented Apr 7, 2017

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

Fixed in c4941a9

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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@TSC21
Copy link
Member Author

TSC21 commented Apr 7, 2017

We still need to consider #529

@TSC21 TSC21 closed this Apr 9, 2017
@TSC21 TSC21 reopened this Apr 9, 2017
@TSC21
Copy link
Member Author

TSC21 commented Apr 9, 2017

@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 TSC21 closed this Apr 14, 2017
@TSC21 TSC21 reopened this Apr 14, 2017
@TSC21 TSC21 closed this Apr 15, 2017
@TSC21 TSC21 reopened this Apr 15, 2017
@TSC21
Copy link
Member Author

TSC21 commented Apr 15, 2017

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 TSC21 closed this Apr 17, 2017
@TSC21 TSC21 reopened this Apr 17, 2017
@TSC21 TSC21 closed this May 5, 2017
@TSC21 TSC21 reopened this May 5, 2017
@TSC21
Copy link
Member Author

TSC21 commented May 5, 2017

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 TSC21 closed this May 10, 2017
@TSC21 TSC21 reopened this May 10, 2017
@TSC21 TSC21 closed this May 11, 2017
@TSC21 TSC21 reopened this May 11, 2017
@TSC21 TSC21 closed this May 15, 2017
@TSC21 TSC21 reopened this May 15, 2017
@TSC21
Copy link
Member Author

TSC21 commented May 15, 2017

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.

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 ;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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;

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yeah. more reasonable

@TSC21
Copy link
Member Author

TSC21 commented May 16, 2017

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
Copy link
Member Author

TSC21 commented May 27, 2017

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).

Copy link
Member

@vooon vooon left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did you tested?

@@ -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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not latching type?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what do you mean with latching type?

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)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Wtf here with tab/space?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can't tell. uncrustify made this

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We need more mess with fill_xyz()!

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

cleaned

@@ -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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

And altitude is still AMSL.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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


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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Used once.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ok sure

Copy link
Member Author

@TSC21 TSC21 Jun 6, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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;


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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also used once.

@TSC21
Copy link
Member Author

TSC21 commented Jun 6, 2017

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.

@vooon vooon merged commit 864115a into mavlink:master Jun 7, 2017
@vooon vooon added this to the Version 0.20 milestone Jun 7, 2017
@TSC21 TSC21 deleted the global_origin_offset branch June 7, 2017 08:22
TSC21 added a commit to TSC21/mavros that referenced this pull request Aug 19, 2017
…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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants