GPS waypoint navigation demo

[SteveMacenski]

Using RL nav sat transform to create an outdoor GPS waypoint following demo, both with and without fusing in some other mapping methods (v-slam, lidar slam, localizers, etc).

The case of with SLAM is for increased accuracy and having a map to for a planner to work in if the waypoints are too sparse in a complex environment for the local planner to reliably navigate. If you're in a maze and the waypoints are only defining the end goal, you will probably need a planner to route through the maze to follow the GPS.

If you're in an open space, the waypoints are dense, or there's a straightline view from each waypoint, then a planner is largely unnecessary and SLAM wouldn't be much additional help, beyond additional robustness in positioning to GPS drift.

For regular interval waypoints in potentially massive (1kmx1km) dynamic environments where its not realistic to map, have the global costmap be rolling with size sufficient to capture the current and next goals to plan within to get around large obstacles that the controller may not be able to reliably navigate around. The rolling costmap for global then allows you not to have to map this massive space or have a full 1km x 1km costmap in memory.

---

Rel. links:
https://docs.swiftnav.com/wiki/ROS_Integration_Guide
https://answers.ros.org/question/218137/using-robot_localization-with-amcl/

[SteveMacenski]

@rotu is this something you guys at Rover would be interested in working on? It would be both a good advertisement for your platform and show using navigation2 and robot localization to accomplish some outdoor GPS guided task (without or with a pre-generated map). At that point write up a little Nav2 website page about it / tutorial?

I have alot of toys laying around, but this is the antithesis of the toys I have (all robots differential indoor, no GPS, but tons of lasers, but no map required...)

[SteveMacenski]

Some additional notes:

• For robot localization configurations: should GPS be the "main" and have SLAM be differential? Or flipped? (or no differential or all differential?)
• Poke someone on GPS+SLAM in ROS world; recommended configs folks use
• GPS jumping - how to tune or become more tolerant to.
• Using RTK and/or D-GPS
• 3 Demos: GPS demo alone of static sized space, dense waypoint following; GPS demo with rolling global costmap for planning towards points; GPS + SLAM.
• Using RTK with nav2 / RL. Sounds like RTK with a base station fix removes the jumping / drifting issues. And you can even piggy back on a nearby station that's not even yours if within 5-6 miles.
• Recommend using dual band (or only support)

[jediofgever]

I am interested in this. Preferably without SLAM initially. I do not have hardware on my hand for now(GPS or LIDARS). I will have in future though.
At the moment, I rely on gazebo_plugins to decorate my robot with required sensor suite. I do have custom created ackermann drive robot with a nice outdoor gazebo world that I copied from the gazebo_models.

I am determined to dive deep in this however I had better have someone that can collaborate with me, @SteveMacenski maybe you could summarize the high level architecture of classes/packages to realize a suitable integration of this with navigation2

[SteveMacenski]

For completeness from a post on the Slack, the 3 demos I had in mind are as follows:

• 1. Navigating dense GPS points (I don't know, lets say every 10 cm or every 1-2 meters, something that a local trajectory planner could follow without needing any additional information from a map to do longer-term planning between points, if they were say 20-30 meters apart in stead). This would make it so that there was no required SLAM / map. This is the most pure of the GPS waypoint navigation. Potentially / optionally have the waypoints sent to the controller as the path, if dense enough to mimick one in place of a path planner.
• 2. Navigating sparse GPS points in a reasonable sized environment - so has a map / SLAM (waypoints are not close together, so there must be some level of planning between them, potentially taking into account the map of the space if the waypoints don't have visibility between each other [e.g. within line of sight straight lines so no map required]). This creates the tech demo of "fusing" GPS data with a map to navigate around a campus, building, etc that is mappable. Uses a global costmap of fixed since by static map.
• 3. Navigating sparse GPS points in a unsizable environment - so no map / SLAM possible and can't have fixed sized costmap (waypoints are not close together and it is not possible to map the environment due to its sheer size or scope). This creates the tech demo of having to fuse GPS with a rolling global costmap of minimum size distance between waypoints, so that it can create plans and follow them for a GPS navigation task, avoiding obstacles placed into the costmap based on live sensor data. This would be the 'I'm driving across a desert' demo, unmappable or of such size that SLAM and processing a costmap of that size is not practical. The rolling costmap size is set such that the current robot pose and the next GPS waypoint are always in the costmap so that we can create plans, processing current data, and follow them to that GPS marker.

---

What might be your timing on hardware? This task is 3 fold: creating something that works doing this, documenting setup and process to enable it, and demonstrations that its working. It should be possible given the state of the stack now to do it, its mostly about documenting it so users have it as a 'feature' and showing it work in the real world in those 3 primary situations (though you personally aren't required to do all of them, I'd just like those 3 to be documented at some point)

The only package you need to use for this is navigation2 and robot_localization

[jediofgever]

All 3 demos are on my list , though like I said, I had like to move slow and clear . Let's focus on the first demo for now.

What might be your timing on hardware?

Say 2-3 months from now. I am quite confident that if we make things work within simulation, it will take little time to test on real hardware(I will have access to a ready to roll outdoor robot). About making demo public(videos etc.), I will confirm with my affiliation but I guess it will be possible.

creating something that works doing this,

Do we have any diagram/scheme of components that are going to be involved to do first demo ?
I am aware that people do outdoor navigation by tricking navigation to use an empty map,
Then following a series of conversions
GPS raw latitude,longitude ==> UTM ==>x,y in map frame
Finally setting and navigating to this x,y as a goal with navigate_to_pose action. Is this the best we can do here ❓
Please correct me if I am mistaken, with the current navigation2 we will at least need to include a Life-cycle Node that will read waypoints and do the above conversions and finally execute them one by one.

It should be possible given the state of the stack now to do it,

Almost , but I think at the moment we need the above process, but I agree there isn't much lacking for navigation2 to do this.

After we have the demos on hand , documenting isn't that hard, Though documenting it to where ? navigation.ros.org ?

[SteveMacenski]

Got it - yes documenting on navigation.ros.org

If you were running SLAM, you'd have SLAM publishing a odom->map transform that nav uses to position. If you had localization with AMCL, the same thing. Basically, all you need to do is replace the TF publisher for that transform from a "lidar thing" to a "gps thing".

That "gps thing" is robot localization. It has something called the navsat transform that takes in IMU / GPS data and provides a map-framed pose. Then with an instance of the robot_loclaization EKF, we smooth that and publish the appropriate transform. See 'dual navsat ekf' config files in robot_localization for examples of that (1 EKF for odometry, 1 EKF for GPS smoothing, 1 NavSat for processing GPS fixes).

Poof, things should work. Then the question on costmap_2d is around how to size it. Typically, lidar slam will give you a static map which sets the size for the map for use in costmap_2d. Now that you don't have a static map setting that size, you have to use the width / height / rolling parameters (depending on which demo we're doing at the time) to set the size of space for the robot to operate in / roll around. Then the rest of things work exactly the same, the obstacle/voxel layer will process obstacles in the scene into the costmap for use in planning / control to get you to your goals. The inflation layer inflates. Just now there's no static layer for the case of no-SLAM so you're having to navigate based solely on the measurements of the environment.

[jediofgever]

I configured the robot_localization for the simulated robot with the GPS(Not that perfect if robot is climbing on a hill) using the dual EKF navsat_transform_node.
Also see this PR cra-ros-pkg/robot_localization#611 in robot_localization
So I have holy chain of map -> odom -> base_link now.

my Rough thoughts on costmaps are;
for the global_costmap, set the width height, resolution to some very large int after that I think we should not be worrying about the global_costmap.

For the local_costmap, something like (40,20,0.2) respectively for width height, resolution and rolling_window enabled should be good enough.

should this local_costmap be at odom or map frame ?, robot_localization suggests that we execute local path plans and motions in odom frame, as that is the continuous transform. But I think I need to hear more on this to spot right frame to be used for costmaps.

[SteveMacenski]

Well, you don't want it to be something hilariously large because then you might have to update that hilariously large thing or have a massive strain on your memory. Since the static map essentially sizes you costmap for your application, this is now a designer parameter for you to set it based on your application's needs.

If, for instance, your application has waypoints that are 10 cm from each other, then your costmap really doesn't need to be very much larger than your local costmap, just to be able to get around obstacles in the way. Maybe 10x10 meters, but that's just a guess. The aims of your local costmap haven't changed, so I'd keep that around the same size / resolution as your typically would for a comparable SLAM application.

Local costmap should be in odom. The real question here is if the global costmap should be in map or odom frame and whether it should be rolling. It all depends on your waypoint distance and needs. Can you highlight for us what you're actually trying to do that falls in line with this demo? See above for my 3 canonical examples, do any of those describe your aim (and if not, in those terms, what is your aim)?

[jediofgever]

I have started from zero to integrate and adapt robot_localization, navgation2 and the robot model itself. So it took quite some time. But now I have all these 3 components doing what they supposed to do and have all essential things set up to work together.
From the moment I am writing this , I am looking into clarifying costmaps , their sizes and frames deeply and I will update here with my findings on what parameters will work best for the first use case(Navigating dense GPS points).
I think 10 cm would be too dense that so I am thinking of 1m between each waypoint.

The real question here is if the global costmap should be in map or odom frame

my current thought is that better be at map, as GPS wouldn't accumulate slips/shifts from wheel causing odom to shift over time.

Can you highlight for us what you're actually trying to do

The task I am trying to do here is make the robot follow a given set of GPS waypoints in a outdoor field. The robot is aimed for farming tasks such as plant treatment with UV light or grassing moving etc..

the routes will be given in forms of GPS waypoints,and will be drawn by a human.
This will correspond to the first use-case I think. But this is just the initial task. As we have basic functions like GPS waypoint following then the other use-cases will be on the list

---

---

---

Edit;

I did an initial waypoint test, though it isn't GPS waypoint yet. Just set the waypoints with usual RVIZ plugin to check response.
very short video here;
I will write a node to take in GPS waypoints, convert them to map frame and call on FollowWaypoints to the same test.

A few other things;
I had to keep global costmap also in odom frame, as the map frame is shaky and discrete, I will still need to play with robot_localization parameters.

In the test
global costmap ; I set the size quite large but with poor resolution(1000 , 1000 , 2 -> height , width, resolution ) so it didn't(hopefully) occupy large chunks in memory, also with its layers disabled.
The reason is I would like the reachabilty of the robot to be large for a given GPS waypoint,

local costmap ; for obstacles and local paths I rely to meaningful sizes of local costmap. Now they are (10 , 10 , 0.1 -> height , width, resolution )
But like you said these, these params depends the density and distance requirements for GPS waypoints for the use-cases.

[SteveMacenski]

Any questions there? Its good to hear your process but just making sure you didn't expect some specific response :-)

What's wrong with map frame? It can be a little jumping, that's fine. 1 meter distance between pts is also pretty dense. It's just not dense enough that you could send the waypoints as the path to the controller directly so that there wouldn't largely be a need for a path planner.

[jediofgever]

In simulation environment the GPS waypoint following is now working(with pure GPS points long, lat,alt). It is on flat gazebo worlds, I haven’t test on uneven hilly worlds.
Thanks to
https://github.com/cra-ros-pkg/robot_localization/blob/79162b2ac53a112c51d23859c499e8438cf9938e/src/navsat_transform.cpp#L114
I could easily transform GPS waypoint to map frame and navigate through them.

ATM there is planning between waypoints. Waypoints are no less than 1m.

However In the first use case it mentions;

Navigating dense GPS points (I don't know, lets say every 10 cm or ...

It’s So dense that it won’t require a global planning hence directly feeding the waypoints to controller.

So maybe what I have done so far falls into second use-case that you mentioned above? As there is planning in between.

If that’s the case I could create denser GPS waypoints and call uponFollowPath, but this will I guess require the source of GPS waypoints to be somewhat continuous as the controller will be quick to consume waypoints and expect new ones. Any comment on how to deal with that ?
One thing in my mind is take in all waypoints to program and put them into a queue and execute them consequently.

[SteveMacenski]

I updated some of the descriptions, I think that I didn't add enough / correct information. 100% my bad. I agree, demo 1 needs to be better defined in intent. I updated 2 and 3 to be better defined. 2 was to mean that it is a reasonable sized environment, so we can have a SLAM map augmenting GPS (demo 2 is tl;dr GPS + SLAM or GPS + map). 3 was to mean its so large, we can't possible map it nor can we have a fixed-sized costmap since its so large (demo 3 is tl;dr GPS + rolling costmaps).

So I think to close the loop on all the options, demo 1 would be GPS + fixed-sized costmap and no SLAM (tl;dr GPS + fixed sized costmap: the most basic non-SLAM, non-large, known-sized space demo). I don't know what I was trying to get across with the density of points. I think separately of these 3 situations there's a question of waypoint density. By that I mean

• Situation A: dense waypoints where they're close enough that the clear use is to follow this path exactly. So something like every 10 cm or something. In this case, planning is useless, we should just have a planner plugin that passes or takes in the set of waypoints and issues a path of them.
• Situation B: sparse waypoints, where sparsity is anything from like 1 meter to 1 km, doesn't really matter. Either way, required a costmap of sufficient size to cover the robot's current location and the next waypoint and need some kind of path planner to get us to the next waypoint.

As you can tell, those concepts of waypoint density and the situations involving rolling costmap / SLAM in conjunction of GPS are totally decoupled and unrelated. Lets just ignore the density discussion for right now. The "dense" situation is also where we want to follow a dense waypoint defined route, not waypoint following (stopping at each). That's an entirely different task that is probably actually a new planner plugin. I added that separate topic of a GPS path following to the algorithms ticket #1710. This is separate of this ticket which is just GPS waypoint following.

Sorry for conflating those concepts. I dont think that was well flushed out in my mind until now.

[jediofgever]

No worries, at the current progress, the definition of 3 these demos hasn't effected any efforts. The demos involving SLAM will be later on my side, at the moment I aiming navigation into the wild, it doesn't help to do SLAM because of the size and characteristic of area that I would like to cover. But there will be SLAM on the upcoming stages, and that is after I have GPS waypoint following set up reliably. Right now it works as best effort.

If we leave the density of GPS points aside,
I guess that what I have so far falls into 3rd use case then. (Because no SLAM + rolling costmaps+ Pure GPS waypoint following in the wild)
Although there are minor issues , this use case(3RD) is mostly ready to be documented and demonstrated in Gazebo Simulation environment.
The current 2 issues are ;

• The robot model I have is ackermann, I have configured smac_planner for it, (nice plans indeed 👍🏼 ) but In the current form my robot performs pretty bad when taking turns, its control related and can be addressed.
• robot_localization; this has been hard to deal with. issue is; map frame sometimes explodes, it happens time to time, I have gone through all the parameters, so far i couldn’t figure out why would odom frame be so stable but not the map frame
  this is only reason I have not achieved
  “ GPS waypoint following set up reliably.“ yet, because of map frame shakes, it also effects the conversion of LLtoMAP. After this gets solved I will update here.