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

TSC21 · 2017-04-15T11:19:18Z

The purpose is to use the GeographicLib to compute:

Geoid to Ellipsoid height and vice-versa (only done on global_position.cpp plugin for now)
Change UTM to ECEF and compute the local coordinates to publish the odometry
Update HomePosition.msg and add height conversion

I'm not very happy to include a build for it as an ExternalProject on CMakeList.txt, so probably I will add a dependency only to geographiclib-tools deb pkg and then add a command on CMakeList.txt to install the datasets.

The lib also allows to compute magnetic fields and gravity field for each coordinate, so we can probably add some usage for that in sensors.

TSC21 · 2017-04-15T12:23:01Z

Update: So seems that geographiclib-tools is a different repo than GeographicLib. I'm updating the CMakeList.txt so it doesn't always have to compile and install the GeographicLib repo by adding two cmake Modules to find any trace of GeographicLib.

…eginning each compile time

TSC21 · 2017-04-15T14:46:20Z

Update: Ok I'm more happy with the result now. Since I don't have a GPS module available, I'm going to try to publish fake gps coordinates and see what do I get.

vooon · 2017-04-16T06:48:38Z

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;


I don't like that.

will change

vooon · 2017-04-16T06:49:36Z

mavros/CMakeLists.txt

+          ./tools/geographiclib-get-geoids -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data egm96-5 &&
+          ./tools/geographiclib-get-gravity -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data egm96 &&
+          ./tools/geographiclib-get-magnetic -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data emm2015
+  )


Should be better solution. And Why use external project???

it's user independent. so if there's no GeographicLib detected, it installs it

This is horrible. I wouldn't want to install ~500MB of stuff just for this to work, and anyway most embedded targets on a drone may will not have internet connections.

This change will greatly kill the portability and lightweight nature of mavros.

vooon · 2017-04-16T06:50:26Z

mavros/src/plugins/global_position.cpp

-		fix->altitude = msg.alt / 1E3;		// m
+		fix->altitude = msg.alt / 1E3 +		// conversion from height abov geoid (AMSL)
+					GeographicLib::Geoid::GEOIDTOELLIPSOID	// to height above ellipsoid (WGS-84)
+					* egm96(fix->latitude, fix->longitude);		// in meters


Why egm96 is local?

local variable you say? does it make a difference?

TSC21 · 2017-04-17T21:10:21Z

@vooon are we good to merge this?

mhkabir · 2017-04-18T09:29:51Z

mavros/CMakeLists.txt

+          ./tools/geographiclib-get-geoids -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data egm96-5 &&
+          ./tools/geographiclib-get-gravity -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data egm96 &&
+          ./tools/geographiclib-get-magnetic -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data emm2015
+  )


This is horrible. I wouldn't want to install ~500MB of stuff just for this to work, and anyway most embedded targets on a drone may will not have internet connections.

This change will greatly kill the portability and lightweight nature of mavros.

TSC21 · 2017-04-18T09:46:40Z

This is horrible. I wouldn't want to install ~500MB of stuff just for this to work, and anyway most embedded targets on a drone may will not have internet connections.
This change will greatly kill the portability and lightweight nature of mavros.

It's not 500 not even close. The geoid dataset is around 20mb. And I'm replacing this with the deb package. Just have to find a way around to install the dataset.

TSC21 · 2017-04-18T10:03:50Z

Just to clarify datasets sizes so there's no misunderstanding:
egm96-5 - 19Mb (Geoid Model) (0.1 mm of max cubic error - 1 μm accuracy)
egm96 - 2.1Mb (Gravity field Model)
emm2015 - 4.3Mb (Magnetic field Model)
The last two can be used to compute gravity and magnetic fields for a certain place, so they can be useful to assert over sensor noise.

…dinates

khancyr · 2017-06-10T17:05:13Z

@TSC21 Hello, I test on odroid C1+ with Ubuntu 16.04 and ROS Kinetic connect to ardupilot SITL copter on another computer.
With the geographic lib rostopic hz /mavros/global/positon/global
average rate: 4.160
min: 0.195s max: 0.274s std dev: 0.01558s window: 100
Without
average rate: 4.167
min: 0.153s max: 0.358s std dev: 0.02093s window: 100
I try serveral time and the results were similar. It doesn't seems to have a performance impact.

Two things I notice at install : updating from source install need a rosdep update ! and libgeographic-dev is needed as dependency. It could be nice to write it somewhere for those (like me ) that rarely use rosdep install

I also test with my PC and no other problem noticed

TSC21 · 2017-06-10T18:37:46Z

@TSC21 Hello, I test on odroid C1+ with Ubuntu 16.04 and ROS Kinetic connect to ardupilot SITL copter on another computer.
With the geographic lib rostopic hz /mavros/global/positon/global
average rate: 4.160
min: 0.195s max: 0.274s std dev: 0.01558s window: 100
Without
average rate: 4.167
min: 0.153s max: 0.358s std dev: 0.02093s window: 100
I try serveral time and the results were similar. It doesn't seems to have a performance impact.

Those are great news! Thanks for sharing @khancyr!

Two things I notice at install : updating from source install need a rosdep update ! and libgeographic-dev is needed as dependency. It could be nice to write it somewhere for those (like me ) that rarely use rosdep install

That will happen if you have already the package installed from src and require updating. If you follow a fresh install from src, following what we have on the install instructions, it will install without any problems. But I can add it to the README.md file as a reminder.

I also test with my PC and no other problem noticed

That's perfect! Thank you!

TSC21 · 2017-06-10T18:40:22Z

@khancyr BTW are you able to share the resulting output, with and without GeographicLib, from rostopic echo /mavros/global_position/global and rostopic echo /mavros/global_position/local please? Just to be sure it is using the geoid dataset the right way. Thank you!

khancyr · 2017-06-10T18:58:36Z

I will look at it tomorrow, from memory on SITL default location the use of geo lib give 604m on altitude and 584m without. It should be better to check with a real gps as 584m is the default altitude of SITL, I think .
I will look at that tomorrow or monday

TSC21 · 2017-06-10T19:00:12Z

I will look at it tomorrow, from memory on SITL default location the use of geo lib give 604m on altitude and 584m without. I should be better to check with a real gps as 584m is the default altitude of SITL.
I will look at that tomorrow or monday

Thank you! I think it should be very similar. But in any case always better to test in a real GPS. BTW, instructions updated on f6e6d7c.

vooon · 2017-06-11T04:48:57Z

@khancyr can you compare load averages, top?

TSC21 · 2017-06-11T16:59:39Z

@vooon,
On my computer (I7 4th gen, 32gb RAM):
top - mavros_node with GeographicLib running:

  PID USER      PR  NI    VIRT    RES    SHR S  %CPU %MEM     TIME+ COMMAND                                                                                                                           
 4977 tsc21     34  14 1028604  74048  28384 S  11.3  0.2   4:27.39 mavros_node

it varies between 10.9 and 13%. Load average: 0.29

without:

  PID USER      PR  NI    VIRT    RES    SHR S  %CPU %MEM     TIME+ COMMAND                                                                                                                           
 7870 tsc21     20   0  991796  33352  28492 S  10.7  0.1   0:07.06 mavros_node

varies between 9.7 and 12.3%. Load average: 0.16

So I don't see a significant change, at least for mavros running on a laptop. Let's check @khancyr results if he's able too.

TSC21 · 2017-06-15T12:09:55Z

@khancyr you were able to test the load balance on your Odroid-C1? Please when you can let us know of your results so we can safely merge this.

khancyr · 2017-06-16T09:40:49Z

laptop (i5-4200H 8gb ram) using SITL on default sim_vehicle.py -v ArduCopter --console --map -w . Mode guided, takeoff 10, and just wait at 10m
Without :
virtual memory : 949,8MB, cpu <2%, average : between 0.7% and 1.3% MEM 0.3%
rostopic hz /mavros/global_position/global
average rate: 4.169

        min: 0.230s max: 0.269s std dev: 0.01283s window: 103
header: 
  seq: 329
  stamp: 
    secs: 1497605788
    nsecs: 622204977
  frame_id: base_link
status: 
  status: 0
  service: 1
latitude: -35.3632622
longitude: 149.165237
altitude: 594.0
position_covariance: [1.4641000923156753, 0.0, 0.0, 0.0, 1.4641000923156753, 0.0, 0.0, 0.0, 5.856400369262701]
position_covariance_type: 1

rostopic hz /mavros/global_position/local

average rate: 4.165
        min: 0.228s max: 0.271s std dev: 0.01385s window: 100
pose: 
  pose: 
    position: 
      x: 696719.896024
      y: 6084519.42821
      z: 9.99
    orientation: 
      x: 0.0032032351323
      y: -0.00018209859428
      z: -0.754678135288
      w: -0.656087355649
  covariance: [1.4641000923156753, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.4641000923156753, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.856400369262701, 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]
twist: 
  twist: 
    linear: 
      x: 0.05
      y: -0.06
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0

With :
virtual memory : 1000,4MB, cpu <2% : average : 1.3% MEM 0.9%
rostopic hz /mavros/global_position/global

average rate: 4.164
        min: 0.222s max: 0.275s std dev: 0.01417s window: 100
header: 
  seq: 1327
  stamp: 
    secs: 1497605653
    nsecs: 344362251
  frame_id: base_link
status: 
  status: 0
  service: 1
latitude: -35.3632612
longitude: 149.1652358
altitude: 613.448636366
position_covariance: [1.4641000923156753, 0.0, 0.0, 0.0, 1.4641000923156753, 0.0, 0.0, 0.0, 5.856400369262701]
position_covariance_type: 1

rostopic hz /mavros/global_position/local

average rate: 4.165
        min: 0.229s max: 0.271s std dev: 0.01349s window: 100
pose: 
  pose: 
    position: 
      x: -0.178974575702
      y: -0.121082689343
      z: 10.0
    orientation: 
      x: -0.00361492856864
      y: 0.000259280485207
      z: -0.754825086672
      w: -0.655916117804
  covariance: [1.4641000923156753, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.4641000923156753, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.856400369262701, 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]
twist: 
  twist: 
    linear: 
      x: -0.07
      y: 0.08
      z: 0.0
    angular: 
      x: nan
      y: nan
      z: nan

TSC21 · 2017-06-16T09:44:26Z

@khancyr are yoi able to run top on your Odroid C1 and check the "load average" when running the mavros_node with and without this PR please?

khancyr · 2017-06-16T09:46:40Z

@TSC21 yep . I am doing it now ! I separate the test to make it easier to write and read ^^

khancyr · 2017-06-16T09:51:28Z

odroid C1+
Without :
virtual memory : 141MB cpu : average 1.3% MEM 1.1%

With :
virtual memory : 188MB cpu : average 2.7% MEM 6.8%

TSC21 · 2017-06-16T09:55:26Z

Are those the results of running top on the command line?

khancyr · 2017-06-16T09:55:26Z

Results we got a little more CPU usage and lot more memory usage

khancyr · 2017-06-16T09:56:07Z

I use htop as I don't know how to show only one process on top ...

TSC21 · 2017-06-16T09:57:03Z

The memory is normal because it requires loading the datasets to memory. But since they are small, the increase is not so relevant (5% on the odroid).

TSC21 · 2017-06-16T09:59:17Z

But when you run top or htop there is a field in the top of the command line that appears saying "load balance". Can you share it's value please?

khancyr · 2017-06-16T10:02:12Z

odroid
Without:
load average: 0,07, 0,08, 0,06
with :
load average: 0,08, 0,08, 0,06

TSC21 · 2017-06-16T10:04:10Z

Without:
load average: 0,07, 0,08, 0,06
with :
load average: 0,08, 0,08, 0,06

Is that on the Odroid C1 or the laptop?

khancyr · 2017-06-16T10:04:28Z

on odroid

TSC21 · 2017-06-16T10:06:43Z

Seems a small difference but if it is like that it is awesome. Thanks for the results!
@vooon as you can see there's no relevant performance modifications (if there is at all). So I think we are good to go

…mavlink#693) * first commit * Check for GeographicLib first without having to install it from the beginning each compile time * add necessary cmake files * remove gps_conversions.h and use GeographicLib to obtain the UTM coordinates * move conversion functions to utils.h * geographic conversions: update CMakeLists and package.xml * geographic conversions: force download of the datasets * geographic conversions: remove unneeded cmake module * dependencies: use SHARED libs of geographiclib * dependencies: correct FindGeographicLib.cmake so it can work for common Debian platforms * CMakeList: do not be so restrict about GeographicLib dependency * global position: odometry-use ECEF instead of UTM; update other fields * global position: make travis happy * global position: fix ident * global_position: apply correct frames and frame transforms given each coordinate frame * global_position: convert rcvd global origin to ECEF * global_position: be more explicit about the ecef-enu transform * global position: use home position as origin of map frame * global position: minor refactoring * global position: shield code with exception catch * fix identation * move dataset install to script; update README with new functionalities * update README with warning * global_position: fix identation * update HomePosition to be consistent with the conversions in global_position to ensure the correct transformation of height * home|global_position: fix compile errors, logic and dependencies * home position: add height conversion * travis: update to get datasets * install geo dataset: update to verify alternative dataset folders * travis: remove dataset install to allow clean build * hp and gp: initialize geoid dataset once and make it thread safe * README: update description relative to GeographicLib; fix typos * global position: improve doxygen references * README: update with some tips on rosdep install

TSC21 force-pushed the geographic_calc branch from d3782c9 to 5868291 Compare April 15, 2017 11:21

first commit

5868291

Check for GeographicLib first without having to install it from the b…

76e6d13

…eginning each compile time

TSC21 force-pushed the geographic_calc branch from ff91492 to 1d70505 Compare April 15, 2017 13:20

add necessary cmake files

1d70505

TSC21 force-pushed the geographic_calc branch 3 times, most recently from bc2c50c to 0a768e1 Compare April 15, 2017 14:32

TSC21 mentioned this pull request Apr 15, 2017

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

Merged

2 tasks

TSC21 added the enhancement label Apr 15, 2017

TSC21 self-assigned this Apr 15, 2017

TSC21 mentioned this pull request Apr 15, 2017

GPS and global_position altitude (NavSatFix - WGS84; MAVLink - AMSL) #529

Closed

TSC21 added the testing required label Apr 15, 2017

vooon requested changes Apr 16, 2017

View reviewed changes

TSC21 closed this Apr 17, 2017

TSC21 reopened this Apr 17, 2017

mhkabir requested changes Apr 18, 2017

View reviewed changes

TSC21 force-pushed the geographic_calc branch from 1215466 to fca06ed Compare April 18, 2017 14:34

remove gps_conversions.h and use GeographicLib to obtain the UTM coor…

79239ee

…dinates

TSC21 changed the title ~~Use GeographicLib tools to guarantee ROS msg def and enhance features~~ [WIP] Use GeographicLib tools to guarantee ROS msg def and enhance features Apr 18, 2017

TSC21 force-pushed the geographic_calc branch 3 times, most recently from ece5286 to b6d5ac8 Compare April 18, 2017 18:57

move conversion functions to utils.h

88fe78e

TSC21 force-pushed the geographic_calc branch from 3582aee to 88fe78e Compare April 18, 2017 20:05

TSC21 force-pushed the geographic_calc branch from 2d2a539 to 3d021dc Compare June 9, 2017 14:57

TSC21 mentioned this pull request Jun 9, 2017

setpoint_raw/global not working? #728

Closed

global position: improve doxygen references

8e03a70

README: update with some tips on rosdep install

f6e6d7c

vooon merged commit 99c8a0a into mavlink:master Jun 19, 2017

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

vooon added the API change label Jun 19, 2017

TSC21 deleted the geographic_calc branch June 22, 2017 19:03

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

TSC21 commented Apr 15, 2017 •

edited

TSC21 commented Apr 15, 2017

TSC21 commented Apr 15, 2017

vooon Apr 16, 2017

TSC21 Apr 16, 2017

vooon Apr 16, 2017

TSC21 Apr 16, 2017

mhkabir Apr 18, 2017

vooon Apr 16, 2017

TSC21 Apr 16, 2017

TSC21 commented Apr 17, 2017

mhkabir Apr 18, 2017

TSC21 commented Apr 18, 2017

TSC21 commented Apr 18, 2017 •

edited

khancyr commented Jun 10, 2017 •

edited

TSC21 commented Jun 10, 2017

TSC21 commented Jun 10, 2017

khancyr commented Jun 10, 2017 •

edited

TSC21 commented Jun 10, 2017

vooon commented Jun 11, 2017

TSC21 commented Jun 11, 2017 •

edited

TSC21 commented Jun 15, 2017

khancyr commented Jun 16, 2017 •

edited

TSC21 commented Jun 16, 2017 •

edited

khancyr commented Jun 16, 2017

khancyr commented Jun 16, 2017

TSC21 commented Jun 16, 2017

khancyr commented Jun 16, 2017

khancyr commented Jun 16, 2017

TSC21 commented Jun 16, 2017 •

edited

TSC21 commented Jun 16, 2017 •

edited

khancyr commented Jun 16, 2017 •

edited

TSC21 commented Jun 16, 2017

khancyr commented Jun 16, 2017

TSC21 commented Jun 16, 2017

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

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

Conversation

TSC21 commented Apr 15, 2017 • edited

TSC21 commented Apr 15, 2017

TSC21 commented Apr 15, 2017

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 Apr 17, 2017

Choose a reason for hiding this comment

TSC21 commented Apr 18, 2017

TSC21 commented Apr 18, 2017 • edited

khancyr commented Jun 10, 2017 • edited

TSC21 commented Jun 10, 2017

TSC21 commented Jun 10, 2017

khancyr commented Jun 10, 2017 • edited

TSC21 commented Jun 10, 2017

vooon commented Jun 11, 2017

TSC21 commented Jun 11, 2017 • edited

TSC21 commented Jun 15, 2017

khancyr commented Jun 16, 2017 • edited

TSC21 commented Jun 16, 2017 • edited

khancyr commented Jun 16, 2017

khancyr commented Jun 16, 2017

TSC21 commented Jun 16, 2017

khancyr commented Jun 16, 2017

khancyr commented Jun 16, 2017

TSC21 commented Jun 16, 2017 • edited

TSC21 commented Jun 16, 2017 • edited

khancyr commented Jun 16, 2017 • edited

TSC21 commented Jun 16, 2017

khancyr commented Jun 16, 2017

TSC21 commented Jun 16, 2017

TSC21 commented Apr 15, 2017 •

edited

TSC21 commented Apr 18, 2017 •

edited

khancyr commented Jun 10, 2017 •

edited

khancyr commented Jun 10, 2017 •

edited

TSC21 commented Jun 11, 2017 •

edited

khancyr commented Jun 16, 2017 •

edited

TSC21 commented Jun 16, 2017 •

edited

TSC21 commented Jun 16, 2017 •

edited

TSC21 commented Jun 16, 2017 •

edited

khancyr commented Jun 16, 2017 •

edited