Navigation2 for Small Robots

Earlier this year I built a small robot for the RoboGames tablebot competition. You can read about that robot in my previous post. I’ve now decided to update that robot for Robogames 2024, and also have the robot tackle the fire fighting competition.

Hardware Upgrades

I competed in this fire fighting competition about a decade ago, using a robot with just an 8-bit AVR microcontroller for a brain and a bunch of dedicated sensors. This time around, I’m going to leverage ROS 2, Navigation2, and a thermal camera.

To accomplish this, I’m using a low-power Celeron-based computer. It’s smaller and lower power than any of the NUC computers I’ve used before and runs right off 12v. I added a second battery to support the computer.

I 3D-printed an entirely new chassis for the robot, in fire engine red. The neck assembly contain the LD-06 lidar, and will soon have the thermal camera and a fan to extinguish the candle:

I also built a mock up of the arena. I didn’t quite have an 8’x8’ area for the arena, so I scaled everything to 7’x7’. In the real competition the hallways will be 2” wider, so I expect that navigation will work better in the real thing.

Step 1: Build a Map

The first step once the robot was mostly assembled was to build a map. The standard approach in ROS 2 is to use slam_toolbox.

Normally, this works pretty much out of the box. But the default parameters are all tuned for full size buildings, and my entire arena is about the same width as a typical hallway.

First change was to reduce the resolution of the map. I initially tried to map with a 1 centimeter grid, however this seemed to cause issues because there is a decent amount of noise in the low cost laser scanner I am using. A grid scale of 2 centimeters caused the scan matcher to perform significantly better without overly risking the map narrowing the hallways and doorways.

With the scan matcher now working, I began to map - but the map consistently exploded about half way through. I pulled up the visualization of the pose graph in RVIZ, and realized that we were getting false loop closures everywhere. The default loop closure search size is 2.0 meters - that’s as big as the whole arena. Turning this way down allowed the map to complete mostly intact.

Step 2: Localization

With a map in hand, attention is turned towards localization. There are a number of parameters to tune here, and so visualization is a needed tool. Nav2 uses a new, custom message for publishing the particle filter poses - so it is important to install the nav2_rviz_plugins package.

With the particle filter poses visualized, I set about making a few changes:

• Since the map is so small, and the robot moves so slowly compared to a full size robot, I reduced the update_min_d parameter so that localization will be updated frequently enough.
• I drastically reduced the alpha parameters for the motion model. Since the robot is quite small, the errors are also quite small. To get good values for the alpha parameters, I basically tweak the settings until the particle cloud diverges just the right amount - not too much, but not too little either. I’ve frequently seen people tune the parameters to be too tight, leading to a lack of particle diversity, which can cause the cloud to converge to an incorrect value and never get back to the correct pose.
• Since we are using the likelihood model, only two parameters are used for the probabilistic model: z_hit and z_rand. Since the environment won’t change during the competition, and there are pretty much no unknown obstacles, these can be set quite aggressively. z_hit is set to 0.98, and z_rand is 0.02 - this basically encodes that there is a 2% chance of a measurement that is not based on something in the map.

At this point, localization was somewhat working, but not entirely reliable. Starting to dig into the scoring a bit more, I reduced the value of laser_likelihood_max_dist thinking that would make a big difference since the default is 0.2 meters and my hallways are only about 0.35 meters wide. But this had negligible effect. In reviewing the classic blue book (Probabilistic Robotics by Thrun, Burgard and Fox), I noticed that I wasn’t actually changing the correct parameter. sigma_hit was what really needed adjustment since it controls how fast the probability drops off. Dropping this from 0.2 to 0.025 made a huge improvement in localization.

A few random remarks:

• In reviewing the blue book I noticed that pretty much every implementation of AMCL out there (and there are now several) don’t treat unknown space outside of the map the way the book does. Every implementation propagates the Gaussian values in both directions from the wall, rather than only going into the free space that was raytraced during the map building phase. It would seem that in some environments, this change could actually help with localization accuracy.
• In working with such close range data, I started to wonder if maybe there should be some accounting for how long the range measurement is when computing the probabilities. Most of the lasers on the market today specify the error as a percentage of the range measured, and all of them have worse accuracy for longer range measurements.

Step 3: Navigation

The next step is to fire up Navigation2. I setup my config and launch files as a copy of the configuration that I used for the UBR-1 robot navigation and then started adjusting for the application:

• I reduced the costmap size to 0.02 meters to match the map, tightened the goal tolerances, and defined a square-ish footprint for the robot.
• For my local controller, I set the appropriate velocity and acceleration limits and drastically reduced the lookahead distances.

I was not getting the results I wanted at first with the GracefulController, so I tried switching to the new MPPI controller. However, that turned out to not be so easy. The MPPI controller is heavily optimized with vectorization/SIMD instructions - but - I’m on a fairly low power Celeron processor that doesn’t support many of the newer SSE / AVX instructions. I tried to build from source and deactivate these optimizations, but kept running into an illegal instructions and eventually returned to tuning the controller I know.

I will note that the Navigation2 documentation has some great documentation on debugging in complex ROS 2 projects. These notes allowed me to launch the controller server in an xterm window, attached to gdb, and see exactly what illegal instruction I was hitting:

Next Steps

That is pretty much where this project is at right now. I’ve got semi-reliable navigation that isn’t entirely fast yet - but that’s not bad for just a few hours of work. I’m going to continue tuning the navigation while also getting the higher level controls worked out for the robot to complete the fire fighting task.

A ROSCon 2023 Recap

One of the benefits of being on sabbatical is that I don’t have to rush back to work after ROSCon - and so I can sit around and write up some thoughts and themes from the show.

Don’t expect me to tell you what the best talks were - I didn’t actually attend any talks. You are not alone if that seems strange to you, my reasoning is that all the talks get recorded and will be posted in a week or two but walking around talking with people can only be done in person.

Getting New Contributors into Open Source Robotics Software

On the Tuesday prior to ROSCon, PickNik Robotics hosted MoveItCon with more than fifty community members in attendance. The most interesting takeaway came from the opening presentation slides:

Over the past year, 29 of 66 contributors to MoveIt2 were FIRST TIME contributors. This stands in stark contrast to many of the more core ROS packages. I had numerous discussions at both MoveItCon and ROSCon about why MoveIt2 has been so successful in luring new contributors. The consensus was really around how little friction new contributors encounter.

MoveIt2 has a large group of very active maintainers - who especially take the time to help new contributors run through the gauntlet they might encounter. There are a number of maintainer strategies at play for MoveIt2, but here are a few you could apply to your own projects:

• Actively maintain the list of tickets/issues on a project, culling out of date ones.
• Instead of spending time tackling some of the easier issues, tag them “Good First Issue” and then spend that time to help a new contributor to solve the bug or implement a new feature.
• Respond quickly to new PRs. Make sure feedback is constructive.
• Have great documentation, especially around how to setup the development environment.
• Be a cool/glamorous project. Ok, this is a bit harder for some of the essential open source packages - but it is hard to deny that MoveIt2 and Nav2 aren’t both benefitting at least a bit from this glamour.

During the closing at ROSCon, the audience was asked to raise their hand if this was their first year at ROSCon - and I would say a majority of hands went up. I’m sure some of them might be long time users or developers of ROS - but I wonder how many more of those people we could help to contribute to ROS over the next year?

The Kids Are, Well, No Longer Kids

There have been 12 ROSCon/ROSWorld events. The folks that have been there from the beginning are getting a bit older. We don’t stay out as late after the conference, and some of us have likely forgotten what it was like just starting out in ROS or robotics. If you have been around in ROS and robotics for a while, here’s some ideas for you:

• Mentor some junior engineers. Connect them to people working on similar problems. Point them at all that documentation you wrote years ago - you wrote documentation, didn’t you?
• You don’t have to just write code - review some code. Help junior developers get their contributions merged.
• Don’t burn out - take care of yourself physically and mentally - there were more than a few people I talked to who aren’t actually as old as they feel - but much of robotics is still taking place in fast-paced startups, and they can take a toll on you…

I certainly wish I had better paid attention to that last point when I was at Fetch Robotics. Since leaving Fetch, I’ve lost 85 pounds and am probably in the best shape of my life - and know I could have been a much better leader and mentor at Fetch if I had not been burnt down most of the time.

What About Those New Faces?

If you are relatively new to ROS or robotics, you’re not alone - the community is still growing fast. I had a number of people at both MoveItCon and ROSCon ask about what they should be studying or learning. My answer evolved a bunch over the course of the week - here’s what I landed on:

• Find some really good mentors. ROS, and robotics in general, moves quicker than most university programs can evolve. I’ve been fortunate to have some really great mentors throughout my career, it is probably the biggest benefit from my Willow Garage days.
• Contribute some open source. Help out an existing project, or even find something that is missing and go make it exist. At Cobalt Robotics, I encouraged my younger colleagues to get involved and in addition to various bug fixes to ROS packages, they have also published some pretty cool work - including an image assembler for Livox lasers and AR visualization of what your robot sees.

We Are So Back

I cannot even count how many people I talked to who have moved back to the greater Boston area, or even the East Coast in general. Further, so many of them talking about work-life balance. I really want to organize a “ROSCon In The Woods” next summer where we just go for a hike and have a BBQ in like southern New Hampshire. Drop me a message if you’d be interested in such a thing.

The Elephant In The Room

Finally, at ROSCon there was the elephant in the room - the acquisition of Open Robotics by Intrinsic. Not much has really changed in how ROS is operating since the acquisition, but there were still certainly discussions of “what if the level of support and funding goes down?”.

We in the ROS Community can’t actually do anything about that - so instead we should focus on what we can do. Having a large open source project so largely maintained and funded by a single organization is never great - just look at the struggle after the demise of Willow Garage. Instead of worrying about what might happen, this is a great opportunity to convince whomever you work for to contribute more to ROS, with both developer time and money. Everyone can be an advocate for open source robotics software.

Building a Tablebot

There was a shortage of entries in the tablebot competition shortly before the registration window closed for RoboGames 2023. To make sure the contest would be held, I entered a robot. Then I had to build one.

What’s A Tablebot?

A tablebot lives on the table. There are three “phases” to the competition:

• Phase I: Build a robot that goes from one end of a table to the other and back.
• Phase II: Have the robot push a block off the ledge of the table.
• Phase III: Have the robot push the block into a shoebox mounted at the end of the table.

There is also an unofficial Phase IV - which is to fall off the table and survive. I did not attempt this phase.

The majority of tablebots are quite simple - a couple of sonar or IR sensors and they kind of wander around the tabletop in hopes of completing the different phases. My tablebot is decidedly different - and it paid off as the robot won the gold medal at RoboGames 2023.

Robot Build

The entire robot is built of 3d printed parts and random things I had on hand.

I’ve had one of those $99 LD-06 lidars sitting around for a while, and decided this was a great project to use it on. I used a Dynamixel AX-12 servo to tilt the laser so I can find the table, the cube, or the goal.

All of the code runs on an STM32, on my custom Etherbotix board which was designed for my Maxwell robot a number of years ago. The robot uses differential drive with some 30:1 12V gear motors, which were purchased from Lynxmotion in 2008 and used in various fire fighting robots over the years.

A set of small digital Sharp IR sensors are used as cliff sensors. These can be moved up or down to calibrate for different table surfaces using a pair of adjustment screws. While the sensors are very accurate and stop the robot, they don’t see far enough ahead when going at full speed, and so I also use the laser to detect when the table edge is approaching.

Phase 1 Software

Phase 1 is pretty straight forward - and mostly based on dead reckoning odometry:

• The laser is angled downwards looking for the table. This is done by projecting to the scan to 3d points, and filtering out anything not in front of the robot at roughly table height. When the table disappears (number of points drops too low), we reduce our maximum speed to something that is safe for the cliff sensors to detect.
• While the laser sensors look for the end of the table, the robot drives forward, and a simple feedback loop keeps the robot centered on the table using odometry.
• When the cliff sensors eventually trigger, the robot stops, backs up 15 centimeters, and then turns 180 degrees - all using dead reckoning odometry.
• The maximum speed is then reset and we take off to the other end of the table with the same behavior.

Phase 2 Software

The movements of Phase 2 are basically the same as Phase 1 - we drive forward, staying centered with odometry. The speed is a bit lower than Phase 1 because the laser is also looking for the block:

• The laser scan is projected to 3d, and we filter out any points that are part of the table based on height. These remaining points are then clustered and the clusters are analyzed for size.
• If a cluster is a good candidate for the block, the robot turn towards the block (using, you guessed it, dead reckoning from odometry).
• The robot then drives towards the block using a simple control loop to keep the heading.
• Once the block is arrived at, the robot drives straight until a cliff sensor trips.
• At that point, the robot stops the wheel on the side of the tripped cliff sensor and drives the other wheel very slowly forward so that we align the front of the robot with the edge of the table - ensuring the block has been pushed off the table.

Phase 3 Software

The final phase is the most complex, but not by much. As with the earlier phases, the robot moves down the table finding the block:

• Unlike in Phase 2, the robot actually approaches a pose just behind the block.
• Once that pose has been reached, the robot tilts the laser back to level and finds the goal.
• The robot then turns towards the goal in the same way it first turned towards the block.
• The robot then approaches the goal using the same simple control loop, and in the process ends up pushing the block to the goal.

All of the software for my Tablebot is availble on GitHub.

Robogames Video

Jim Dinunzio, a member of the Homebrew Robotics Club, took a video during the actual competition at Robogames so you can actually see the winning set of runs:

Visualization

To make development easier, I also wrote a Python GUI that renders the table, the robot odometry trail, the laser data, and detected goals and cubes.

Fun with Math

Along the way I actually ran into a bug in the ARM CMSIS DSP library. I used the arm_sin_cos_f32() function to compute my odometry:

arm_sin_cos_f32(system_state.pose_th * 57.2958f, &sin_th, &cos_th);
system_state.pose_x += cos_th * d;
system_state.pose_y += sin_th * d;
system_state.pose_th = angle_wrap(system_state.pose_th + dth);

This function takes the angle (in degrees!) and returns the sine and cosine of the angle using a lookup table and some interesting interpolation. With the visualization of the robot path, I noticed the robot odometry would occasionally jump to the side and backwards - which made no sense.

Further investigation showed that for very small negative angles, arm_sin_cos_f32 returned huge values. I dug deeper into the code and found that there are several different versions out there:

• The version from my older STM32 library, had this particular issue at very small negative numbers. The same bug was still present in the official CMSIS-DSP on the arm account.
• The version in the current STM32 library had a fix for this spot - but that fix then broke the function for an entire quadrant!

The issue turned out to be quite simple:

• The code uses a 512 element lookup table.
• For a given angle, it has to interpolate between the previous and next entry in the table.
• If your angle fell between the 511th entry and the next (which would be the 0th entry due to wrap around) then you used a random value in the next memory slot to interpolate between (and to compute the interpolation). At one point, this resulted in sin(-1/512) returning outrageous values of like 30.

With that bug fixed, odometry worked flawlessly afterwards. As it turned out, I had this same function/bug existing in some brushless motor control code at work.

Robogames Wrap Up

It is awesome that RoboGames is back! This little robot won’t be making another appearance, but I am starting to work on a RoboMagellan robot for next year.