When I started working on my own TurtleBot variant (before I even decided to call it Phoebe) my intention was to build a hardware platform to get first hand experience with ROS fundamentals. Phoebe’s Hackaday.io project page subtitle declared itself as a ROS robot for <$250 capable of SLAM. Now that Phoebe can map surroundings using standard ROS SLAM library ‘gmapping‘, that goal has been satisfied. What’s next?

One disappointment I found with existing ROS SLAM libraries is that the tutorials I’ve seen (such as this and this) expect a human to drive the robot during mapping. I had incorrectly assumed the robot would autonomously exploring its space, but “simultaneous location and mapping” only promises location and mapping – nothing about deciding which areas to map, and how to go about it. That is left to the human operator.

When I played with SLAM code earlier, I decided against driving the robot manually and instead invoked an existing module that takes a random walk through available space. A search on ROS Answers web site for something more sophisticated than a random walk resulted in multiple pointers to the explore module, but that code hasn’t been maintained since ROS “groovy” four versions ago. So one path forward is to take up the challenge of either update explore or write my own explorer.

That might be interesting, but once a map is built, what do we do with it? The standard ROS answer is the robot navigation stack. This collection of modules is what gives a ROS robot the ability to plan a path through a map, watch its progress through that plan, and update the plan in reaction to unexpected elements in the environment.

At the moment I believe it would be best to learn about the standard navigation stack and getting that up and running on Phoebe. I might return to the map exploration problem later, and if so, seeing how map data is used for navigation will give me better insights into what would make a better map explorer.

(Cross-posted to Hackaday.io)

The Roboclaw ROS node is responsible for calculating odometry information based on encoder values read from each wheel. In order to translate them into standard ROS units, it needs two parameters:

• ticks_per_meter : To calculate physical distance traversed by each wheel, the code needs to know how many encoder counts it takes for the wheels to travel one meter.
• base_width : To calculate how much the robot has turned, the code needs to know how far apart the two drive wheels are placed.

Both of these values needed updating after upgrading Phoebe to the second chassis. I got impatient with the slow speed of the first draft, so the motor gearboxes were changed out with ones that deliver less torque and precision but much faster top speed. This change of gearing would need a new ticks_per_meter value. And the second chassis is slightly wider than the first, which obviously changes the base_width value. Both of these could be calculated on paper, but that is only a starting point. Real world is always a little different from the theoretical and needs a little adjustment.

The easiest place to start is ticks_per_meter. Phoebe is placed on a flat surface, next to a ruler, and commanded to drive straight forward for a short distance. During this activity, the odometry data is monitored with rostopic echo /odom to see how far Phoebe thinks it has actually gone. If the ruler said Phoebe didn’t go as far as it thought it did, increase ticks_per_meter. If Phoebe overshot, reduce that value.

Once wheel travel was verified with a ruler, LIDAR is added to the picture. RViz is commanded to plot odometry data and LIDAR data together, and Phoebe is placed facing a door serving as a large flat surface for reference. The red arrow represents where Phoebe thinks it is, facing the horizontal line representing its LIDAR’s view of the door.

Then Phoebe was commanded to move backwards 0.5m. If odometry data agrees with LIDAR data, the movement away from door and the door’s distance from robot would match, canceling each other out so the line representing the door would not move on the RViz plot. It looks like the ruler calibration worked out well, as we’re only a tiny bit off.

Once distance was verified, we move on to rotation. Command Phoebe to make a 90 degree turn clockwise and see if LIDAR plot agrees. Again, ideally the turn calculated from odometry would agree with the LIDAR plot, leaving the door in roughly the same place on the RViz plot. Ideally.

In this case, however, the door shows a minor clockwise rotation. This change of position in LIDAR data indicates Phoebe didn’t turn as far as it thought it did. To adjust parameters so Phoebe’s calculations better align with actual motion, we can increase the base_width parameter. And if the door had rotated the other way (Phoebe turned further than it thought it did) the parameter should be decreased.

(Cross-posted to Hackaday.io)

Now that I’m driving my TurtleBot-derived robot Phoebe around and mapping my house, I’m starting to get some first-hand experience with robotic mapping. One of the most fascinating bits of revelation concerns map resolution.

When a robot launches the Gmapping module, one of the parameters (delta) dictates the granularity of the occupancy grid in meters. For example, setting it to 0.05 (the value used in TurtleBot 3 mapping demo) means each square in the grid is 0.05 meters or 5 cm on each side.

This feels reasonable for a robot that roams around a household. Large objects like walls would be fine, and the smallest common obstacle in a house like table legs can reasonably fill a 5cm x 5cm cell on the occupancy grid. If the grid cells were any larger, it would have trouble properly accounting for chair and table legs.

So if we make the grid cells smaller, we would get better maps, right?

It’s actually not that simple.

The first issue stems from computation load. Increasing resolution drastically increases the amount of memory consumed to track the occupancy grid, and increases computation requirements to keep grid cells updated. The increase in memory consumption is easy to calculate. If we halve the grid granularity from 5cm to 2.5cm, that turns each 5cm square into four 2.5cm squares. Quadrupling the memory requirement for our occupancy grid. Tracking and maintaining this map is a lot more work. In my experience the mapping module has a lot harder time matching LIDAR scan data to the map, causing occasional skips in data processing that ends up reducing map quality.

The second issue stems from sensor precision. An inexpensive LIDAR like my unit salvaged from a Neato robot vacuum isn’t terribly precise, returning noisy distance readings that varies over time even if the robot and the obstacle are both standing still. When the noise exceeds map granularity, the occupancy grid starts getting “fuzzy”. For example, a solid wall might no longer be a single surface, but several nearby surfaces.

As a result of those two factors, arbitrarily increasing the occupancy map resolution can drastically increase the cost without returning a worthwhile improvement. This is a downside to going “too small”, and it was less obvious than the downside of going “too large.” There’s a “just right” point in between that makes the best trade-offs. Finding the right map granularity to match robots and their tasks is going to be an interesting challenge.

(Cross-posted to Hackaday.io)

Once odometry calculation math in the Roboclaw ROS driver was fixed, I could drive Phoebe (my TurtleBot variant) around the house and watch laser and odometry data plotted in RViz. It is exciting to see the data stream starting to resemble that of a real functioning autonomous robot! And just like all real robots… the real world does not match the ideal world. Our specific problem of the day is odometry drift: Phoebe’s wheel encoders are not perfectly accurate. Whether from wheel slippage, small debris on the ground, or whatever else, they cause the reported distance to be slightly different from actual distance traveled. These small errors accumulate over time, so the position calculated from odometry becomes less and less accurate as Phoebe drives.

The solution to odometry drift is to supplement encoder data with other sensors, using additional information can help correct for position drift. In the case of Phoebe and her  TurtleBot 3 inspiration, that comes in courtesy of the scanning LIDAR. If Phoebe can track LIDAR readings over time and build up a map, that information can also be used to locate Phoebe on the map. This class of algorithms is called SLAM for Simultaneous Location and Mapping. And because they’re fundamentally similar robots, it would be straightforward to translate TurtleBot 3’s SLAM demo to my Phoebe.

There are several different SLAM implementations available as ROS modules. I’ll start with Gmapping because that’s what TurtleBot 3 demo used. As input this module needs LIDAR data in the form of ROS topic /scan and also the transform tree published via /tf, where it finds the geometry relationship between odometry (which I just fixed), base, and laser. As output, it will generate an “occupancy grid”, a big table representing a robot’s environment in terms of open space, obstacle, or unknown. And most importantly for our purposes: it will generate a transform mapping map coordinate frame to the odom coordinate frame. This coordinate transform is the correction factor to be applied on top of odometry-calculated position, generated by comparing LIDAR data to the map.

Once all the pieces are in place, Phoebe can start mapping out its environment and also correct for small errors in odometry position as it drifts.

SLAM achievement: Unlocked!

(Cross-posted to Hackaday.io)

I’m making good progress on integrating ROS into my TurtleBot variant called Phoebe. I’ve just configured RViz to plot Phoebe’s location as reported by Roboclaw ROS driver‘s odometry information topic /odom. On the same plot, I’ve also placed data reported by Phoebe’s scanning LIDAR. Everything looked good when I commanded Phoebe to drive forward, but the geometry went askew as soon as Phoebe started turning. The LIDAR dots rotated one way, and the odometry reporting arrow rotated another. Something is wrong in the geometry calculation code.

For this particular problem I knew exactly where to start looking. When I assembled Phoebe, I knew I needed to configure motors in the way expected by the Roboclaw ROS driver. The README didn’t explicitly say which motor went on which side, so I went into the source code and got conflicting answers. In the motor driving routine (cmd_vel_callback) it performed calculation for right wheel motion and sent it to motor 1, and left wheel motion went to motor 2.

However, this doesn’t match encoder calculation code. The method EncodeOdom::update_publish expects its parameters to be (enc_left, enc_right). In method Node::run, it retrieves encoder values from motor 1 and saves it in variable enc1, and enc2 for motor 2, then calls update_publish(enc1, enc2). Which would treat encoder value of motor 1 as enc_left instead of enc_right, the opposite of what we’d want.

So the fact data on /odom is going the wrong way was not a surprise, but why didn’t the LIDAR transform also suffer the same problem? This was traced down to an earlier change submitted as a fix for the reverse transform problem – that commit added a negative sign in front of the calculated angle when broadcasting odom -> base_link transform. But that only masked and didn’t fix the underlying problem. The transform might look right, but /odom data is still all wrong, and variables like self.last_enc_right is actually holding the left side value.

The correct fix would be to reverse the parameter order when calling EncodeOdom::update_publish which correctly assigns encoder 1 count the right motor, and encoder 2 the left motor. And with the underlying problem fixed, we no longer need the negative sign so that can be deleted.

With this fix, Phoebe’s laser plot and odometry plot in RViz appear to agree with each other and both correspond correctly to the direction Phoebe is turning. I’ve submitted this fix as a pull request on the Roboclaw ROS driver.

Now that the Roboclaw ROS node is configured for Phoebe TurtleBot’s physical layout, and the code is running reliably instead of crashing, I can drive it around as a remote control car. This isn’t the end goal, of course. If all I wanted was a remote control car there were far cheaper and easier ways to get here. Right now the goal is just to verify functionality of the drive system and that it is properly integrated into the ROS infrastructure.

In order to send driving commands to Phoebe, I need something that publishes to the command & velocity topic /cmd_vel. On an autonomous robot, the navigation code would be in charge of sending these commands. But since it’s a pretty common task to move a robot around manually, there are standard ROS packages to drive a robot via human commands. The most basic is teleop_twist_keyboard which allows driving by pressing keys on a keyboard. Alternatively there is teleop_twist_joy for driving via a joystick.

Those remote control (or ‘tele operation’ in ROS parlance) nodes worked right off the bat, which is great. But I quickly got bored with driving Phoebe around on the carpet in front of me. First I launched RViz to visualize scanning LIDAR data as I did before. This was enough for me to drive Phoebe beyond my line of sight, watching surroundings in the form of the laser scan dots. After I verified that still worked, I stepped up the difficulty: I wanted RViz to plot laser data on top of odometry data in order to verify that Roboclaw ROS node is generating odometry data correctly.

To do this, I needed to participate in the ROS coordinate transform stack, the infrastructure to track all the frames of reference for relating robot components to each other in physical space. The Roboclaw ROS node publishes a transform to relate a robot’s position reference frame base_link to the odometry origin reference frame odom. The LIDAR ROS node publishes its data relative to its own neato_laser reference frame. As the robot builder, it was my job to write a transform to relate neato_laser frame to Phoebe’s base_link frame. Fortunately, ROS transform tutorials covered this exact task and I quickly got my desired RViz plot.

It looks like the LIDAR scan plot from earlier, but now there’s an arrow indicating Phoebe’s position and direction. The bigger change not visible in this screen shot is that RViz is now plotting in the odometry frame. This means we no longer watch strictly from Phoebe’s viewpoint  where robot stays in the center of screen. The plot is now in odometry frame, and Phoebe should be moving relative to the map.

I drove Phoebe forward, and was happy to see the laser scan stayed still relative to the map and the red arrow representing Phoebe moved forward. But when I started turning Phoebe, the red arrow turned one way and the LIDAR plot moved the opposite way.

Something is wrong in the coordinate transform data, time for some debugging…

I now turn my attention to finding the root cause of random sporadic failures when the Roboclaw ROS driver makes calls into Roboclaw API. The most common symptom of these failures is the TypeError I addressed earlier, but avoiding crash by TypeError is only a band-aid. There were still other issues. Sometimes Phoebe’s movement stutters. And even more mysteriously, sometimes when Phoebe was supposed to be standing still, it would twitch for a fraction of a second.

Looking into ROS logs, the node never intended to send a movement. But it does send a continuous stream of “run at speed X” commands at a rate of ten per second, whether it is trying to move the robot or not. When staying still, that stream continues at the same rate constantly sending “run at speed zero”. The fact that my robot twitches when it’s supposed to be standing still tells me that “run at speed zero” command is occasionally corrupted into a “run at speed X” command. Which starts the motor moving for 1/10th of a second until it is stopped by the next non-corrupted “run at speed zero” command.

Any time there is a random sporadic failure, my instinct is to look for places where threading collisions may be taking place. Programming for multi-threaded environments can get tricky. When I wrote code for SGVHAK Rover, the intent was to make that code easy to understand and pick up. In that spirit, I explicitly kept everything on a single thread to avoid multi-threading issues.

But now we’re playing in the major leagues and there’s no avoiding multiple threads. ROS itself runs across multiple processes and threads, so it could scale to robots that have multiple on board computers, each of which has multiple processing cores. Fortunately, ROS implementation takes care of almost everything, each ROS node just has to make sure they’re doing the right thing with their own private data.

There are at least three different ROS topics involved in this Roboclaw ROS node:

1. Driving commands received via /cmd_vel
2. Odometry computation broadcast to /odom
3. Diagnostics information

Each ROS topic is processed in own thread, so given three topics, we should expect at least three different threads who might all call into Roboclaw API at the same time to perform their tasks. Armed with this knowledge, I looked for code managing cross-thread access. My plan was to review that code to see if I can find any obvious problems with it.

That plan was changed when I found no code managing cross-thread access. I guess its absence qualifies as an obvious problem and it would certainly explain the kind of behavior I saw.

Not being a Python expert, I cruised StackOverflow for a pattern I could use to implement Python thread synchronization. I decided it was most straightforward to use the with keyword described on one of the later replies on this “Semaphores on Python” thread. Using this pattern makes the code change delta very straightforward to read.

There were a few initial calls into Roboclaw API to set things up, I left those alone. But as soon as the code started kicking off events that would have other threads (specifically, when it started the diagnostics thread) every following call into Roboclaw API is synchronized by a threading.Lock() object. With this modification, we can guarantee that only one thread will be performing serial communication to Roboclaw motor controller at any given time, and avoid data corruption by multiple threads trying to talk to the serial port at the same time.

Phoebe ran smoothly and reliably after this work. No more stutter in motion, and no more twitching when standing still. I’ve submitted the fix as a pull request.