Tag: NAV2

2023 03 25 Raven Status Update

A fair amount of progress as been made on Raven over the last few days.

I have list of TODO items that need to be tackled. One of those items has been on the list or months and this week it bubbled to the top. My proximity sensor values were being time stamped with old values.

In ROS (Robot Operating System), the whole software stack that tries to generate commands to move the robot somewhere interesting relies on a flood of data coming in, like SONAR readings, time of flight readings and LIDAR readings. Each of those readings comes with a timestamp indicating when the reading was taken.

On my robot, the readings are made on a custom PC board I made, and then sent to the main computer of the robot. The custom board needs to have the same sense of “now”-ness as the main PC — the two computers have to agree within a few milliseconds on what time it is now. It doesn’t do any good to take a SONAR reading and then have it show up a half second late to the main computer. The algorithms all have a small “tolerance” for time skew, and if sensor readings are too old, they are rejected out of hand as useless.

The software piece I use to transmit my sensor values from the custom board to the main computer is a package called “Micro ROS”. I use this software with some small customization as my custom board has a lot more capability to it that the usual, intended board using Micro ROS. 

One function built into Micro ROS is the ability to ask another computer what time it is, and set its own clock to match that other computer. But setting it just once doesn’t work. Each computer has a clock driven by a crystal-controlled oscillator, and crystals drift as they heat up. Worse, much worse, the CPU clock chip in my custom board seems to “burp” now and then when interrupt signals come in and my hardware generates a fair number of interrupt signals.

Another problem is that Micro ROS has baked into it an expectation that the main computer is using a particular version of communication software, and the expected version currently has bugs which cause my stereo cameras to not operate correctly. It took a bit of reading for me to realize that little factoid.

For the moment, I can ignore that, so I set my main computer back to using the buggy communication software. Also, when Micro ROS asks for the current time, it takes “quite a bit of time” to get the result, usually about 150 milliseconds, but sometimes as much as a second. So any answer it gets from the main PC will be inherently wrong right away.

My last few days of programming have been devoted to finding someway to make that all work with an allowable tolerance for errors in timing. I tried over and over, and I’m normally very good at finding creative solutions to tricky problems. Still my code got progressively worse the more I tried to fix the problem. And then, my robot buddy Ralph Hipps called for one of our at-least-daily robot chats and in the process of explaining the problem to him, it occurred to me what the root cause was. My custom board was attempting to do a complex operation during interrupt handling.

Interrupt handlers on a computer must be very quick. If your interrupt handler code takes more than about a millisecond, sometimes even only a few tens of microseconds, everything falls apart. And because I was explaining the symptoms to someone else, I finally realized that I was taking probably tens of milliseconds in my interrupt handler for the SONAR sensors.

Once I realized that, the fix was easy. The details aren’t too important, but I stashed the important information from the signal in a shared, global location and exited the interrupt handler. Then the normal, non-interrupt code picked up the stashed information for processing. Outside of the interrupt handler, you can largely take whatever time you want to process data. Sort of. ROS navigation depends heavily on high “frame rates” from sensors. If you can’t generate, say, LIDAR data 10 times a second or faster, you have to slow your robot way down, otherwise the robot would move faster than it could react to, say, the cat rearing up in front of your robot with nunchucks in hand.

The robot is now happily sending sensor data nearly three times faster than before, about 80 frames a second and rarely gets the time out of sync by very much. Now I can move on to the new problems that have shown up because I fixed this problem.

Below is an obligatory snap shot of a part of the visualization software showing my robot, in a room, with LIDAR, SONAR and time of flight sensors showing as well. No cat is in this picture.

Everything about robots is hard (TM)

Robot Footprints in Nav2 for ROS 2

Don’t forget to include a footprint in your Navigation 2 (Nav2) configuration file so that the local and global cost maps can properly account for the outline of the robot when it is attempting to avoid collisions. You need a footprint for both the local and global cost maps. My cost map parameters are store in a file called “nav2_params.yaml“, and within it there is a section for “local_costmap” and one for “global_costmap“. Here is snippet from the configuration file showing the footprint line for the local cost map for my robot.

local_costmap:
  local_costmap:
    ros__parameters:
      footprint: "[[-0.450, -0.215], [-0.450, 0.215], [0.235, 0.215], [0.235, -0.215]]"

And there is a similar “footprint” line in the global_costmap section.

The footprint value must be enclosed in quotes, has an outer pair of square brackets, and then is a list of x and y comma-separated coordinate pairs, each pair enclosed in square brackets and each bracketed pair separated by commas. Each x and y coordinate is a point in the base_link coordinate frame of the robot.

You don’t need to add a coordinate pair at the end back to the origin–it will be assumed. So a line segment from the last pair [0.235, -0.215] to first pair [-0.450, -0.215] is assumed in the above example.

If you’re rather, you can declare a circular outline for your robot instead. Using, e.g.,

robot_radius: 0.25 # The outline is a circle with radius 0.25 meters

You don’t nave to use the same foot print list or radius for both the local and global cost maps. You might, for instance, use a footprint list for the local cost map but a radius for the global cost map. The article referenced at the end of this post has a discussion about this.

Here is what the above footprint outline looks like in rviz2 when visualizing my robot (which is also showing LIDAR, SONAR and time-of-flight sensors and the local cost map). The lime-green square box in the center is the footprint.

See Setting Up The Robot’s Footprint for more information.

2022 11 02 Puck Status

The big result since the last status report is that more, very important safety systems have been implemented to protect the robot and its surroundings, and the robot now has more data about objects near to it as it moves so it can avoid running into those obstacles. And the new power and data control panel is finished, which makes it easier for me to disassemble the robot.

The last week and a half has seen a lot of work on Puck. Two of the time of flight sensors were replaced as the originals were either not sending distance measurements or the measurements were wildly wrong. While that layer of the physical robot case was opened to replace the sensors, I also color-coded all the cables for the time of flight and SONAR sensors, making it easier to trace the cables from the custom micro processor board (a.k.a the Teensy Monitor Board) to the physical sensors. The new power and data control panel was finished and all the new wiring is in place.

I also rewired the signal cable from the relay block to the emergency stop of the motor controller. This would allow the Teensy Monitor to immediately stop power going to the motors if some emergency situation were discovered.

A lot of work was done in the software of the Teensy Monitor. More diagnostic messages were added, which showed that there are still reliability issues with communication between the Teensy Monitor and the sensors, which is something I’ll have to solve eventually. I may just need to add better cable shielding.

The motor temperature sensors now are published to a topic, and the temperature values are now averaged as the sensors tend to be rather noisy. Code was added to detect an over-temperature situation for the motors. When the robot is driven for a long time, the temperature inside the motors can build up to the point that there is a risk of melting the wires inside.

Importantly, code to detect a runaway motor situation is now implemented. This happens especially when the encoder connections to the motor controller come loose. Remember that the robot vibrates a lot when it moves, and the connectors on the RoboClaw motor controller are not designed to stay well connected in the face of vibration.

The redundant motor current sensors were found to be useless, so the code that talked with those sensors is disabled for now. I’ll need to find some better sensors in the future. Meanwhile the motor current sensors built into the RoboClaw are working well enough. And code was added to detect an over-current situation, which can happen when the robot runs up against, say, a wall and the motor wheels stop turning or turn very slowly. Left in this situation, the wires in the motor would quickly overheat and literally melt.

With the three new fault detection code routines, the robot now get shut off via the emergency-stop signal to the RoboClaw if the motors get too hot, if the motors take off at high speed unexpectedly, or if the motors start drawing a lot of current, beyond normal operation. This is a three way protection to save the expensive motors and prevent the robot from running into something and just keep pushing forward.

With the now fully working SONAR and time of flight sensors, and the data being published at 25 to 30 frames per second, I finally figured out how to hook those sensors into the robot motion planning software. Now, if the robot is moving along and comes across an obstacle that only one of the proximity sensors can see (the LIDAR sees objects only at a specific height), the robot can make a plan to move around the object.

Outstanding work remains to discover why some of the sensor values are occasionally rejected by the planning software. It is likely that is because the data is too old, meaning that the data was more than a couple of milliseconds old by the time the planning software saw it. There may be a simple fix, but it needs to be investigated.

Adding SONAR-like sensors to Nav2

It is probably typical that the Navigation-2 (Nav2) setup under ROS 2 is just using LIDAR for generating costmaps. It is also possible to include SONAR-like sensors in your cost maps, that is sensors that publish a Range type of message. The process is easy.

Edit your YAML parameter configuration file for Nav2 as follows. In the “local_costmap” section, you will find a line that lists the plugins to be used to generate the local costmap. Add a “range_layer” to the list. Following the plugins line, you should see, e.g., sections describing the obstacle_layer and inflation_layer — that is, sections corresponding to the names in the plugins list. You will need to add a new section describing the range_layer.

Note, the names obstacle_layer, inflation_layer, range_layer, etc in the plugins list are arbitrary names, they don’t actually make the layer become, say, and obstacle layer. It’s what you supply as sub-parameters that make the section do what it does.

local_costmap:
  local_costmap:
    ros__parameters:
      plugins: ["obstacle_layer", "inflation_layer", "range_layer"]
      range_layer:
        plugin: "nav2_costmap_2d::RangeSensorLayer"
        topics:
          [
            "/sonar0Sensor",
            "/sonar1Sensor",
            "/sonar2Sensor",
            "/sonar3Sensor", 
            "/tof0Sensor",
            "/tof1Sensor",
            "/tof2Sensor",
            "/tof3Sensor",
            "/tof4Sensor",
            "/tof5Sensor",
            "/tof6Sensor",
            "/tof7Sensor",
          ]
      inflation_layer:
        .......rest of local_costmap YAML.....

Just like the section describing, say, the inflation_layer, you will add a new range_layer section by repeating the name given in the plugins list, followed by a colon. The first entry should describe the class name that will handle the data type. In the case of Range type values, the line should say:

plugin: "nav2_costmap_2d::RangeSensorLayer"

Then add a topics entry consisting of the word topics followed by a colon and then a single topic name in quotes, or a list of topic names formed by a left square bracket, then each quoted topic name in the list separated by commas and a closing right square bracket.

That’s it. If you bring up the nav stack, you should now see that when you wave you hand in front of your SONAR sensor, a new obstacle shows up in the local costmap.

There are a couple of caveats to consider. The published topic containing the Range message needs to have a valid frame_id in the header. And that frame_id needs to be described in the URDF for the robot, which, of course, needs to be published so that the transform system in ROS knows how to relate the Range value to the coordinate frames of the robot.

And, the time stamp in the header needs to be accurate and timely. This especially could be a problem if you are, say, publishing the data from an Arduino micro controller which may not have a notion of the time used by the rest of the ROS components. If you are publishing using micro–ROS, there are ways to synchronize your Arduino with the main ROS system.

There are a few additional parameters besides plugin and topics that you can provide for the range_layer. They are described here.