GSoC 2023: GPS-Denied Autonomous Exploration with ROS 2

Blog
1

image

Introduction

Hi! I’m Pedro Pimentel Fuoco, a 4th year undergraduate student from the Polytechnic School of the University of São Paulo studying mechatronic engineering. I’ve been at the USP autonomous drone team since I started university and last year I was appointed head of software development for 2022. I worked a lot transitioning between ROS and ROS2 during our competitions so contributing to the ROS 2/DDS support in Ardupilot really caught my eye. In the last few months I’ve been helping out by adding data writers on the AP_DDS_Client, which publishes Ardupilot data to ROS 2 topics.

I am thrilled to announce that I’ve been selected for GSoC’23 to work towards my project “GPS-Denied Autonomous Exploration with ROS 2” with the help of the Ardupulot community and my mentors @tridge and @rfriedman.

The current state of the ROS 2/DDS support

Using and contributing to the ROS 2 support is still a challenge, as there are little to no use cases documented. Taking that into account, generating code and documentation about Ardupilot’s integration with ROS 2 serves the interests of the community and allows for more contributions to focus on DDS in the future.

Project Motivation

GSoC 2022 had a very interesting project about non-GPS navigation on copters and
rovers, but one of it’s main problems was pointed in the comments by @soldierofhell: “ROS 1
will be EOL in 2025”, to which @snktshrma, the student responsible for the project,
answered “Shifting to ROS2 and Navigation2 is on the future tasks for this project”. Furthermore, it will be a great addition to have a non-GPS SLAM working and documented
on ROS 2. Replicating similar results to their project should be simpler and faster this year
because of the documentation and scripts created, so it is possible to use the time left
implementing a new and innovative use case to Ardupilot.

Exploration of unknown indoor environments is a major issue in mobile robotics, as it
significantly limits the environments in which completely autonomous robots are able to
operate. Autonomous exploration has gained a lot of attention in the last few years, with lots of applications in search and rescue, pipeline maintenance and cave mapping.

The proposed idea is to keep contributing to DDS support, but focusing specifically
on replicating GSoC’22 non-GPS navigation setup and using it for autonomous exploration in ROS 2, which will help building a documentation foundation for future projects
interested in using DDS instead of MAVROS, as well as expanding the use cases for
Ardupilot with ROS.

As agreed with my mentors, the project will focus on multirotor vehicles, support for other vehicles may be added later.

Project Roadmap

I will update this roadmap with checks and images as the project progresses, so feel free to look into the detailed tasks to know where the project currently is at.
I’ve divided the project in four parts:

Part 0 - Setting up the development environment

Prerequisites:

  • Ardupilot with DDS works in SITL with gazebo
  • Discuss which repository will host this project

Tasks

  • Set up ROS Humble
  • Set up Ardupilot SITL through DDS
  • Install the required ROS 2 packages
  • Setup the simulation in gazebo
    • Get a obstacle map working with an Iris spawned
    • Add the lidar sensor to Iris and get the sensor data published to ROS 2

Part 1 - Mapping and localization

Prerequisites:

Tasks

  • Tune cartographer SLAM to generate odometry and a OccupancyGrid from the lidar data
  • Configure Ardupilot to support ExternalNav
  • Send ExternalNav odometry from cartographer_ros to Ardupilot’s Extended Kalman Filter through DDS
  • Setup rviz2 or Foxglove to visualize both the OccupancyGrid and the odometry output
  • Test the SLAM working on SITL
    • Create scripts to easily reproduce the results of the tests and store them openly at the project repository
    • Record a video of it working properly and how to set it up
  • Document all the details of this process, producing a page for ROS 2 similar to this on the Ardupilot Wiki.

Important Observations
This is an opportunity to improve the process of getting a external-odometry-based application working with ArduPilot
Acceptance criteria for the documentation is someone with not a lot of experience with ROS 2 should be able to run it.
People with ROS1 + Mavlink/Mavros background should have an easy migration experience, as they will likely be the majority of the public using this project.

Part 2 - 2D Path planning and obstacle avoidance

Prerequisites:

Tasks

  • Install the required ROS 2 packages
    • Nav2
  • Porting ap_navigation to work with nav2 and DDS
  • Integrate rviz2 or Foxglove to use “2D Nav Goal” button to set the a position target for the offboard path-planning
  • Send a position (?) and velocity setpoint through the ROS 2 control interface to Copter
  • Use rviz2 or Foxglove to see the CostMaps and Path Planned
  • Test the navigation
    • Create scripts to easily reproduce the results of the tests and store them openly at the project repository
    • Record a video of it working properly and how to set it up
  • Document all the details of this process, producing a page for ROS 2 similar to this on the Ardupilot Wiki.

Important Observations
As a stretch (if time allows it) it would be awesome to support external obstacle sensors data to be sent directly from ROS 2 to Ardupilot, so we could use AP_Proximity over DDS

Part 3 - Autonomous exploration

Prerequisites:
Settle which exploration algorithm will be used (this paper is my initial suggestion)

Tasks

  • Implement Wavefront Frontier Detection algorithm to find a boundary between unexplored and explored space in the OccupancyGrid
  • Iteratively use the boundary points as position targets for the offboard path-planning
    • Create scripts to easily reproduce the results of the tests and store them openly at the project repository
    • Record a video of it working properly and how to set it up
  • Document all the details of this process, producing a page for “autonomous exploration in GPS-denied environments” on the ROS 2 page at Ardupilot Wiki.

Important Observations
If time allows, it would be great to test the same setup of autonomous exploration with a rover, in order to produce more documentation with different vehicles.

Community participation

I’m excited to hear what members of the community and the dev team have to say about the project, feel free to comment suggestions and feedbacks on the comments section below!

In addition to that, as @rfriedman suggested and previous GSoC contributors highlighted during this year’s GSoC Summit, discussions about the work done in this project will be done in public channels, such as blog posts and discord chats, so that members of the community can contribute and help out with their opinions and experiences.

18 Likes
2

@pedro-fuoco so great to have you working on this in GSoC!

1 Like
3

Weekly updates May 23rd

A ~15min weekly meeting will be held at the Ardupilot Discord at every Tuesday 23h00 GMT, feel free to join

This has been settled, a “humble” branch will be created at GitHub - ArduPilot/ardupilot_ros: ArduPilot ROS integration
A PR has been submitted to transform it into an empty ros2 package, so I can begin my work.

Here are some considerations:
Foxglove Studio vs. RViz
TL;DR

  • Rviz requires ROS or ROS 2 installed. Foxglove supports bag playback and has a browser client option for those not willing to download it.
    • This makes it more accessible, but I think it is fair to assume that people interested in this project will have ROS installed

In addition to that it is relevant to mention that the nav2 stack and ardupilot_gz both come with multiple examples that use rviz2.

With that in mind I think it would be better to work with rviz2, as most tools that will be used in this project are already using it.
@rfriedman mentioned adding Foxglove support if I decided to go with rviz2, I could also add it if I have some spare time during the project development (it shouldn’t take long anyway), as it would be nice to be able to run the bags and show the progress in any computer.

Screenshot from 2023-05-22 22-42-29
Screenshot from 2023-05-22 22-42-291294×705 84 KB

Thanks to @rhys for showing me the way. I had some problems to make it work, so I will provide some feedback/contribute to the documentation, as well as start the README for this project’s repo guiding the user on where to find the installation instructions

@arshPratap’s amazing GSoC project will cover this dependencies. If the complete control interface isn’t done in time I can work towards a simpler version with velocity setpoints towards AP guided mode.

4

Notes:

  • ardupilot_ros now has a humble branch: GitHub - ArduPilot/ardupilot_ros at humble
  • ROS Control: Start with copter for now, and move GCS_Mavlink to call into AP_ExternalControl). Need path from DDS to guided mode functions without guided. Need to start with the library, then swap mavlink to use it, and finally add Lua support. Remove centidegrees. The library needs to pick rad/deg, lean towards SI, however user facing. If display or control interface. Example, the attitude message is in radians. Pick one, don’t mix.
  • Pedro leaning towards rviz2 due to other tools use it.

Actions:

  • Pedro to make a PR to move the SDF/world files from ardupilot_ros to ardupilot_gazebo
  • Pedro to see if iris_warehouse has same coordinate issues as ardupilot_gazebo#59 or use walled_iris_runway
  • Ryan to bootstrap initial ardupilot wiki ROS 2 page
  • Ryan to meet with Randy and Tridge and Arsh to hash out a control plane (AP_ExternalControl) and document it. Ryan to write the interface for the control library.
  • For PR’s, test against RVIZ.
1 Like
5

I talked to @rhys , let’s put the URDF models in ardupilot_gz since that already has the launch files. According to Rhys, “ardupilot_gazebo is the SITL /JSON plugin with a minimal number examples to get up and running”.

2 Likes
6

Yes, ardupilot_gazebo doesn’t have a dependency on ROS so it can be run by users with only ArduPilot and Gazebo installed. There are a couple of example models to get up and running, but most of the SDF models live in SITL_Models/Gazebo. This is because the models tend to be large (textures and meshes), and we want to keep the plugin repo light-weight.

ardupilot_gz assumes a ROS install, and will have access to xacro and other packages which allow you to generate SDF from xacro / urdf.

1 Like
7

Very interesting, I am subscribing!

1 Like
8

Interested as well, I would like to know the detailled setup in order to replicate as you know, sometimes we can get stuck with incompatibilities between the different components : Ubuntu - ROS - Gazebo - RVIZ

1 Like
9

Sure thing! Here’s the list of my current setup:

  • Ubuntu 22.04.2 LTS (Jammy Jellyfish)
  • ROS 2 (Humble Hawksbill) and the associated RVIZ version
  • Gazebo Garden
1 Like
10

Very Interesting project i am following as well, just started working with autopilot and also trying to set up a working DDS communication with ROS humble in order to pass odometry msgs , if i will have anything to contribute where should i do this ?

BTW did you choose to use Gazebo Garden and Not Fortress ?

11

A few reasons:

  • Major namespace change between Fortress and Garden which is costly to maintain.
  • Garden has native rendering support for macOS.
  • Better handling of joints in nested models.

The drone community (PX4 and ArduPilot) have settled on the combination of ROS 2 Humble and Gazebo Garden current development.

2 Likes
12

A good starting point is to set up a developer workspace, @rhys is working on it ardupilot_gz. Feel free to join our weekly meeting as well, more details above.

1 Like
13

I understand , make sense overall thank you.

14

Weekly updates June 20th

Coding begins! Now that I’m back from the U.S. (since June 13th) the project should start getting in shape. Progress should be slow until my university break starts (which is when I plan on living and breathing Ardupilot)

I just created a PR in ardupilot_gz to add iris_maze.launch.py, which spawns a copter equipped with a laser scan in a maze-like world and connects them to ROS 2.

Screenshot from 2023-06-20 18-19-51
Screenshot from 2023-06-20 18-19-511003×884 125 KB

Screenshot from 2023-06-20 18-20-39
Screenshot from 2023-06-20 18-20-391606×1021 205 KB

Adding a lidar to a robot in gazebo classic is very well documented, with numerous examples. In Ignition Gazebo there’s basically nothing… This means that even this early implementation might help someone, so I’m happy.

To run this simulation I’ve had to ditch my crappy laptop and create a dev workspace in my desktop computer. I lost some of time with problems installing ubuntu 22.04 in my ssd and with my nvidia graphics card (classic stuff for linux users), but it was very worth it, my real_time_factor in gazebo tripled.

3 Likes
15

Weekly updates June 26th and July 4th

I’ve worked on running Cartographer with the simulation environment and it is currently working pretty well! I still have some tweaks to do regarding launch file options and the tf tree, but it is pretty functional for now. Looking into some of their examples and mimicking their rviz setup is also probably worth it.
I’m also going to add foxglove alternatives for visualization

Screenshot from 2023-07-08 15-30-01
Screenshot from 2023-07-08 15-30-011587×774 119 KB

The simulation PR got merged!

Thanks a lot to @rhys for the huge help to get everything perfect.

I’m currently working towards adding pre-commit to ardupilot_gz

@rfriedman is currently working on the external odometry source prerequisite, I will try to help him out as much as I can. @tridge gave me a masterclass on EKF some weeks ago and I’ve used my notes to provide some info.

The GSoC Midterm Evaluation deadline is on July 14th, I will work towards having a PR to Ardupilot_wiki explaining how to setup the simulation and run cartogrpher on it so it is easily accessible for the whole community.

@Ivan_Rulik is currently working on some Ardupilot/ROS 2 documentation so I will take a look at his work and build from there.

3 Likes
16

July Update

Some small PRs got merged. Thanks to @rfriedman for presenting me to pre-commit and helping me merge this PR to ardupilot_gz

Also updated the README in ardupilot_gz as the Micro XRCE DDS Gen dependency wasn’t listed

This month the first ardupilot_wiki PR regarding ROS 2 was merged, @hwurzburg and @rmackay9 helped me out with it. If you are interested, you can look into the PR or the dev documentation

https://ardupilot.org/dev/docs/ros.html

I’ve had quite some problems to get navigation2 to work properly with the current setup, mostly because of tf tree problems.

In it’s current state, ardupilot_gz is populating the /tf topic using the ros_gz bridge, specifically the gz-sim-pose-publisher-system plugin. What it does is basically create a parent frame with the model name and a bunch of child frames with the model’s links and publish their position relative to each other. This works fine in the iris_runway example, and we can use the /tf information to show successfully the robot position in rviz2.

This is a problem because the tree generated from this plugin serves to simply publish the position of the links of specific models in gazebo, in other words, it doesn’t take into consideration the joint hierarchy. Using cartographer as an odometry source the generated tf tree looks something like this:

ros_gz generated tf tree
ros_gz generated tf tree1568×348 44.5 KB

This is a problem because ROS REP 105 standardizes the tf relationship between frames earth , map , odom and base_link, which causes big open source ROS projects, like navigation2, to assume this kind of tree architecture. It should be as follows:

ros rep 105
ros rep 1051016×259 36.5 KB

Looking into the navigation2 documentation to setup an adequate tf tree, we can see that they don’t use ros_gz and instead choose to go for robot_state_publisher. This results in the following tree:

robot_state_publisher
robot_state_publisher1568×348 46.2 KB

Which looks amazingly better. Using cartographer as a map->odom->base_link source results in this tf tree:

with-cartographer
with-cartographer1568×567 57.7 KB

This tree follows ROS REP 105 standards, which expectedly results in better perfomance with cartographer and navigation2.

Because of all that, I will create a PR to stop ignoring robot_state_publisher’s tf tree and adequate the frame_id’s being published by the sensors in gazebo. @rhys I’d love to hear your thoughts on this.

3 Likes
17

August Update

This has been the most eventful month of the project, full of PRs and updates.

As mentioned in the post above, I submitted a PR to stop ignoring robot_state_publisher and made it “oficial”, remapping it from /ignore/tf to /ap/tf, this will be very important soon.

Google Cartographer had been working with the simulation for some time, but with the /tf changes merged, I could finally submit a PR for adding the Cartographer integration in Ardupilot, which worked beautifully.

In the meanwhile, @rfriedman offered to start working in the library for Ardupilot to accept an External Odometry Source. This was possible before, with AP_VisualOdom, but the “entry point” to this library was on GCS_Mavlink. In other words, it worked for mavlink and ROS 1 (using mavros), but a reform was needed to get it appropriately working in ROS 2, this was masterfully done in the two PRs linked below. Thanks again Ryan!

This was also an amazing opportunity for me to learn how to code review and serve as a code tester for Open Source. I got some suggestions out in the Pull Requests and also debugged code in calls with Ryan. Up until this point I had only been on the submission end of a PR, so having the opportunity to change roles was amazing for my development as a software developer. I also did a PR to fix a bug we found in his branch:

With Cartographer integrated and Ardupilot accepting External Odometry, I could finally work on more advanced documentation, to start, I’ve updated the documentation to teach the end-user how to send the External Odometry into Ardupilot’s Extended Kalman Filter

In ardupilot_wiki a page for ROS 2 with SITL in Gazebo and another for Cartographer SLAM with ROS 2 in SITL were created, with video demos for both.

I added pre-commit to ardupilot_ros, which was very similar to the PR I had done earlier for ardupilot_gz:

Now for the final step of the project: successfully integrating nav2 to work with Ardupilot. This has been a joint effort the last few weeks.
First, we need a way to control the vehicle’s velocity from ROS 2. @tridge kickstarted a PR for AP_ExternalControl, an Ardupilot library to serve as an “entry point” to external control messages, be it from mavlink, scripting or DDS (ROS 2). Initially both me and @rfriedman served as code reviewers, and I was responsible for testing the code with gazebo using ardupilot_gz, we suggested some changes, and Ryan decided to commit a lot of changes to the PR to get it ready for merge ASAP.

Here’s a little gif of me testing the frame orientations.

After that PR was merged, a little alteration was needed. Aerial vehicles with ROS use body frame conventions for velocity, instead of earth-based frames, according to ROS rep 147. On top of that, nav2 uses Twist message instead of TwistStamped. From this incompatibilities, two PRs were created:

Here’s also the link for the discussion surrounding this in navigation2:

I am now working towards succesfully integrating nav2 with Ardupilot. The path planning and speed control parts are working perfectly, but the obstacle avoidance side of things is still not working. This is the last gap in the project.

3 Likes
18

Great Work @pedro-fuoco !

1 Like
19

Done! It works! Finally hahaha
Here is a demo of the copter performing SLAM and Obstacle Avoidance at the same time:

The problem was caused by this issue:

Now the focus is to make it work in the main branches for all the repositories involved, so a bunch of PRs are being created in these last few days. A final update on this post will be made soon.

4 Likes
20

For anyone following, pretty much everything we need is merged aside from the NAV2 integration. The only issue left (in progress by me) is to add support for TwistStamped in NAV2. I made some progress the last week on the code; just waiting on some feedback from the NAV2 maintainers for the TwistSubscriber class.

Then, it will get backported to humble.

Once that’s in, then the final project tickets (wiki and demo) code can get merged:

1 Like