Odometry using Optical Flow

Odometry using Optical Flow

Working with the PAA5100 sensor and ROS 2

A few weeks ago, I got my hands on the PAA5100 Near (15-35mm) Optical Flow sensor from Pimoroni. With such a short range, it seemed perfect for one of my mobile robots, so I decided to write a ROS 2 Humble node for it. I decided to start off using the pmw3901-python library provided by Pimoroni. This library provides classes for both the PMW3901 (long-range) and PAA5100 (short-range) sensors. To things simple, I decided to write the node in Python. I also used this as an opportunity to play around with some ROS 2 concepts in Python - like Lifecycle nodes and Executors, so this implementation is definitely over-engineered.

An optical flow sensor acts like the sensor in a digital mouse, by measuring the movement of objects using the motion of pixels in a sequence of images. By tracking these changes, the sensor can calculate the relative motion of the surface in two dimensions. These relative displacement values are represented in pixels, which need to be converted into meters as will be explained later. The PMW3901 sensor is a long-range sensor (80mm-infinity), which makes it suitable for quadrotors and other flying robots. The PAA5100 has a short range (15-35mm), making it perfect for ground vehicles as the sensor can be placed closer to the ground.

Lifecycle Node

The first step was to write a minimal publisher - I started off by creating a simple optical flow class, and within it creating a node, a publisher, and a timer. I then added a timer callback from where I read the sensor data and publish an empty odometry message. I then created a main function that creates an instance of this class and spins the node.

Next, I updated the node to be a lifecycle (managed) node, using a minimal example here. A lifecycle node uses a state machine to provide control of the state to the user. More information about the lifecycle node can be found in the design documents here. The state machine can be seen below:

To implement the lifecycle node, I first updated the class so that it inherits from (lifecycle) Node class and then added the following five functions:

  • on_configure: this function runs when onConfigure is called. Here, I configure the sensor and create my odometry publisher, transform broadcaster, and timer. If successful, the state of the node transitions from unconfigured to inactive.

  • on_activate: this function runs when onActivate is called. In my implementation, I only use this to log a message stating the sensor is activated. It then transitions the state of the node from inactive to active.

  • on_deactivate: this function runs when onDeactivate is called. Like on_activate, this function logs a message stating the sensor is deactivated and transitions the state from active to inactive

  • on_cleanup: this function runs when onCleanup is called. This function cancels the timer, and destroys the publisher and the transform broadcaster. Then, it logs a message and transitions the state from inactive to unconfigured.

  • on_shutdown: this function runs when onShutdown is called. This function, does the same thing as on_cleanup, except the state transitions from unconfigured, active or inactive to finalized.

I then updated the timer callback, which first checks if the state is active, and if yes, it reads sensor data as delta x and delta y values represented in pixels. These pixel values are first converted to relative distances per time step, using which the absolute distance and the current velocity are calculated. These values are populated in an Odometry message, which is published, and also used to compute the transform between the odometry frame and the base frame. This transform is then broadcasted using the transform broadcaster.

Pixel values -> Distances

It was quite challenging to find the equation to convert from pixel values to distances in meters, especially because of all the guarded information by the manufacturer - more about this later. Eventually, I found two resources - this archived document about the ADNS3080 sensor, and the measurement equations provided in this master thesis. I also realized that the frame of the PAA5100 sensor seems to be rotated 90 degrees clockwise as compared to the default orientation of PMW3901 (according to documentation, not verified), so I had to make the necessary changes. The final equations I ended up using are as follows:

For PAA5100:

$$dist_x = -1 * (\frac{delta_y * h}{res * scaler}) * 2 * tan(\frac{fov}{2})$$

$$dist_y = (\frac{delta_x * h}{res * scaler}) * 2 * tan(\frac{fov}{2})$$

For PMW3901 (untested):

$$dist_x = (\frac{delta_x * h}{res * scaler}) * 2 * tan(\frac{fov}{2})$$

$$dist_y = (\frac{delta_y * h}{res * scaler}) * 2 * tan(\frac{fov}{2})$$

Where h is the height from the ground at which the sensor is placed, res is the resolution of the sensor in pixels, fov is the field of view of the sensor (defined as 42 degrees for both PMW3901 and PAA5100 sensors) and scaler is the scaling factor. delta_x and delta_y are the raw sensor measurements and dist_x and dist_y are the resulting delta displacements in the x and y axes.

Another issue that I observed was the resolution value. The library uses a raw data length of 1225 for both PMW3901 and PAA5100, which is 35x35, i.e res is 35. However, most online resources about the PMW3901 define the resolution as 30x30. This needs to be verified practically with a PMW3901 sensor, which I do not have.

The measured displacements in meters are accumulated every time period to get the absolute distance traveled in the x and y directions, and the displacements are divided by the timer period to calculate the velocity at that particular instance. These values are populated in the Pose (position) and Twist (linear velocity) fields in the Odometry message.

Finally, I updated the lifecycle node to accept constant values like height and resolution using the parameter server. The publisher node can be run using the following command. This command runs the optical_flow_publisher executable that executes the main() function in the publisher node.

$ ros2 run pmw3901_ros optical_flow_publisher

The publisher node implementation can be found here.

Improvements / To Do

There are still some improvements that can be done. Firstly, the publisher assumes that the sensor is placed at a fixed height parallel to the ground (within the range of the chosen sensor variant), and currently does not account for any rotations about any axis. For now, any rotation about the x and y axes would confuse the sensor resulting in invalid measurements. However, these rotations (roll and pitch) can be accounted for using the explanation in the ADNS3080 documentation and the master thesis referenced in the Bitcraze PMW3901 driver. This still needs to be implemented.

Next, the scaling factor - for a single pixel move, the sensor returns a value of more than 1. This is the scaling factor or simply scaler. This scaler seems to be a proprietary value and cannot be found anywhere online, and most references mention an NDA with the sensor manufacturer. This can be calculated empirically by moving the sensor to a known distance and comparing it with the distance measured by the sensor (at a fixed height). It is also unknown if the scaler will change for different sensor heights, but this can only be tested empirically once a sensor mount is designed.

Both PMW3901 and PAA5100 breakout boards have identical footprints and identical positions of the sensor module on the PCB. However, on both these boards, the sensor package is not centered on the PCB. The sensor aperture itself is not centered on the sensor package. This is quite annoying since these offsets must be accounted for when designing the mount for the sensor. Fortunately, there is an accurate mechanical drawing provided by Pimoroni.

Finally, the entire implementation has been tested with the PAA5100 sensor, and not the PMW3901. There are slight differences in the documentation available online, resulting in different resolution values, and different default orientation for PMW3901 as compared to the PAA5100. So, I'm not entirely sure if the implementation will work correctly with the PMW3901 sensor. This needs to be tested practically with a physical sensor.

Executor

With the lifecycle publisher up and running, I decided to implement an executor node. An Executor implements one or more threads of a system to invoke callbacks, and provides control over the execution management, especially with multiple nodes. Some nice examples (for microcontrollers) using C++ can be seen in the execution management documentation for micro-ROS. In my case, since there's only one node, this does not add much value. I only implemented it to see how it is done using the ROS2 Client Library for Python (rclpy).

I found this simple example and used it as a starting point to write my own executor node. This node implements its own main() function, which overrides the one in the optical flow publisher. I've kept the main() in the publisher node just so that I can launch it on its own without using an executor, if it is not required, it can be removed.

The executor node can be run using the following command. This command runs the optical_flow_node executable that executes the main() function in the executor node, overriding the main() in the publisher node.

$ ros2 run pmw3901_ros optical_flow_node

So, now there are two ways to run the node. I am not sure if there is an advantage to using one over the other, especially in this use case. I also haven't run any tests to check. However, it has been a nice learning experience.

The executor node implementation can be found here.

Launch file

So far, I have been running the executor node, which creates an instance of the optical flow publisher in the unconfigured state. I was then configuring and activating the node using the following commands on the terminal:

$ ros2 lifecycle set /optical_flow configure
$ ros2 lifecycle set /optical_flow activate

I wanted to automate the process so that I run the executor node first, then configure the lifecycle node, and finally activate the lifecycle node when the inactive state is reached. This was implemented using a ROS 2 Python launch file which can be found here.

With the launch file working, the package is almost fully implemented. I still haven't implemented any unit tests as I have no experience with testing in Python, but I'll get to it when I get the time. For now, I've sufficiently tested the implementation in various conditions, and it works quite well in a controlled scenario (sensor at a fixed height, parallel to the ground, no rotations).

The optical_flow_ros package can be found here.

ToF Imager

I'm working on a project with the VL53L5CX ToF Imager sensor directly connected to a Raspberry Pi, so I decided to implement a ROS 2 node for it. This is a Time-of-Flight sensor that produces 8x8 or 4x4 range measurements, which can be converted into a pointcloud. So far, I've been using these sensors with a microcontroller in the middle - I connect the sensor to the microcontroller using I2C, populate the pointcloud on the microcontroller and then stream that to the Raspberry Pi using micro-ROS. Two implementations can be seen in the tof_imager_micro_ros package here.

However, in this case, I want it directly connected to the Raspberry Pi's I2C port, so I decided to replicate the optical flow lifecycle node implementation for the VL53L5CX. Similar to the optical flow publisher, the ToF imager publisher first accepts parameters from the parameter server, and uses it configure the VL53L5CX sensor, converts raw sensor measurements into a pointcloud, which is published as Pointcloud2 message. The executor then initializes the lifecycle node and runs it. Finally, the launch file launches the executor node, configures it, and automatically sets the state to active once configured.

The tof_imager_ros package can be found here.

Next up

Next up, I want to set up a small differential drive robot. I've ordered two 100:1 micro metal geared motors from Pimoroni, as well as two of their encoder shields. I will use them with the Inventor HAT that sits on top of a Raspberry Pi Zero 2 W. This robot will use ros2_control to control the differential drive and calculate odometry using the wheel encoders. This odometry will be fused with measurements from the PAA5100 optical flow sensor to get better accuracy. Finally, a camera will be added to the Raspberry Pi.

With the PAA5100 node done, I will next focus my efforts on getting a minimal ros2_control implementation with the differential drive controller. Next, I will come back to the PAA5100 to implement unit tests in Python. I'm still waiting for these motors and encoders to be delivered, I cannot wait to get my hands on them to start implementing the control system. More on this in the next update.

As for my other goal of reading a book every month, I took a break from normal books and picked up a few graphic novels. In April, I read Batman: The Killing Joke and in May, I finished the graphic novel version of 1984, both really good books with some amazing art work. Probably for June, I will pick up another graphic novel but I haven't made up my mind just yet.

Did you find this article valuable?

Support Aditya Kamath by becoming a sponsor. Any amount is appreciated!