ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Using robot_localization, how can I calculate the GPS coordinates of any point on my robot?

asked 2016-12-13 21:16:31 -0500

M@t gravatar image

updated 2016-12-13 21:17:00 -0500

The Problem (TL;DR)

I want to calculate the GPS coordinates of a random hardpoint on my robot, both directly from the GPS and via sensor fusion (EKFs).

The Problem (in full)

This is a conceptual question about the "best practice" for calculating the GPS coordinates of any random hardpoint on my robot using the robot_localization package. I already have something that works, I just want to see if there is a better/more accurate way.

I'm using a Clearpath Robotics "Jackal" with a GPS mounted on top. So far I've been using the excellent robot_localization package to localize the robot using two EKFs. This diagram below shows a simplified version of my frame tree. The first "local" EKF generates the /odom -> /base_link link. The second "global" EKF generates the /map -> /odom link. An instance of navsat_transform generates the /map -> /utm link (note that the /utm frame is located at the 0,0 point of the real UTM grid). The remaining links are set up in the robot URDF. The blue frames are fixed and do not move. The orange frames are fixed relative to the other orange frames but move with the robot and move relative to /map, /odom etc. I want to calculate the GPS coordinates of /hardpoint anywhere on my robot, could be the nose or a mounting point for a manipulator, the exact location isn't critical. The coordinates themselves could be in WGS84 or UTM although UTM is preferred because it's easier for frame transforms.

image description

I would like to calculate the coordinates in both of the following ways:

  1. Directly from the GPS, i.e./auxgps (so if the EKFs go nuts I still know where /hardpoint is)
  2. Via the EKFs (so if I lose GPS lock briefly I still have coordinates for /hardpoint)

I currently have several possible ways of doing this:

  • [GPS Direct Option 1] Use the /auxgps -> /hardpoint. This is more complicated than it sounds because the result has to be relative to the UTM grid. I.e. the /auxgps -> /hardpoint transform by itself is meaningless because it just gives the location of /hardpoint relative to /auxgps, which I already know because I set it in the URDF. I'm a little unsure how to practically achieve this in code (python TF2 documentation isn't great)
  • [GPS Direct Option 2] Calculate the difference between the translational components of the /utm -> /auxgps and /utm -> /hardpoint frame transforms. This will give you a difference in Nth/Est/Elv relative to the UTM grid Subtracting this from the UTM coordinates of /auxgps gives you the UTM coordinates of /hardpoint. Because this uses the difference between frame transforms, any error in the absolute position of /utm should come out in the wash. This method is dirty and more than a little hacky but it's easier to implement than option 1 above.
  • [Via EKF Option 1] Modify the source code of navsat_transform. Navsat_transform publishes a topic called /gps/fix, which is normally the position ...
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted

answered 2016-12-19 04:06:31 -0500

Tom Moore gravatar image

I think your best bet is to write a small node that does a simple tf lookup from hardpoint->utm (which will work, as long as n_t_n is outputting the map->utm transform), and then use the UTMtoLL() method in navsat_conversions.h to get your lat/long.

edit flag offensive delete link more


That's pretty much exactly what I've done and it seems to work pretty well. But I notice that it is very dependent on the accuracy of the EKF that creates /map. If the EKF is wrong, the coordinates are off, hence the need for GPS-direct coordinates. What are your thoughts on that?

M@t gravatar image M@t  ( 2016-12-19 22:00:08 -0500 )edit

I think it's a bit outside the scope of the package, to be honest. What do you mean by "if the EKF is wrong"?

Tom Moore gravatar image Tom Moore  ( 2017-01-04 02:33:15 -0500 )edit

I agree completely, my application is very uncommon but I wanted to ask anyway in case I'd missed something. I haven't done anything to optimize or tune the EKFs, and in open areas they're perfect. But some of my test areas involve sharp slopes near trees, and the EKFs... (1/2)

M@t gravatar image M@t  ( 2017-01-05 01:58:33 -0500 )edit

...Struggle in those are areas (haven’t had time to look into exactly why). The UTM horizontal and vertical position (location of base_link relative to utm) drifts, but the pure GPS position remains more accurate. So not wrong, just less accurate (sorry, my fault for not explaining properly).

M@t gravatar image M@t  ( 2017-01-05 02:00:08 -0500 )edit

If the robot is moving, I would expect the location of base_link relative to utm to change, but I'm guessing you're saying that it drifts even when the robot is still?

Tom Moore gravatar image Tom Moore  ( 2017-01-13 04:27:45 -0500 )edit

Nope, if the robot is stationary the base_link -> utm transform is pretty constant. I'm calculating the location of base_link and hardpoint in a variety of different ways, with accuracy. Comments are too small for this so I will send you an email shortly with a better explanation.

M@t gravatar image M@t  ( 2017-01-15 15:16:20 -0500 )edit

im still confused about [GPS Direct Option 1]. If i have RTK position of gps antenna w.r.t base station, and the gps antenna is on the side of the robot, how do i get the position of the center of the robot w.r.t to the base station?

JadTawil gravatar image JadTawil  ( 2018-08-28 20:50:59 -0500 )edit

Can you please post a new question, @JadTawil?

Tom Moore gravatar image Tom Moore  ( 2018-08-30 07:18:58 -0500 )edit

Question Tools



Asked: 2016-12-13 21:16:31 -0500

Seen: 1,633 times

Last updated: Dec 19 '16