adityakamath / optical_flow_ros

ROS 2 package for PMW3901 and PAA5100 (short-range) optical flow sensors

optical_flow_ros

ROS 2 package for the PMW3901 optical flow sensor and it's short-range variant PAA5100.

Note: This implementation is a bit over-engineered, as I have been experimenting with ROS 2 managed/lifecycle nodes using Python.

Implementation details

• optical_flow_publisher: This executable uses the pmw3901-python library to access sensor data over SPI. The delta X and Y measurements from the sensor are converted to 2D odometry data, which is published periodically using a timer as an Odometry message to the /odom topic and as a transform broadcast to /tf. The odometry publisher uses the Sensor Data QoS profile as default. This implementation is designed as a lifecycle component and can be run individually as well.

• optical_flow_launch.py: This is the launch file that launches optical_flow_publisher as a lifecycle node, loads its parameters, and then configures and activates it. The lifecycle node is first initialized, then set to 'configure' from the launch file. When the 'inactive' state is reached, the registered event handler activates the node.

Parameters

• timer_period: Timer period in seconds (Default: 0.01)
• sensor_timeout: Sensor timeout in seconds in case of no movement, or sensor failure (Default: 1.0)
• parent_frame: Parent frame for the Odometry message and Transform (Default: odom)
• child_frame: Child frame for the Odometry message and transform (Default: base_link)
• x_init: Initial position in the X axis, in meters (Default: 0.0)
• y_init: Initial position in the Y axis, in meters (Default: 0.0)
• z_height: Height of the sensor from the ground, in meters (Default: 0.025)
• board: Sensor type - pmw3901 or paa5100 (Default: paa5100)
• scaler: Scaling factor, i.e. the sensor value returned for 1 pixel move (Default: 5)
• spi_nr: SPI port number (Default: 0)
• spi_slot: SPI CS pin - front (BCM pin 7 on RPi) or back (BCM pin 8 on RPi) (Default: front)
• rotation: Rotation of the sensor in 90 degree increments - 0, 90, 180, 270 (Default: 270)
• publish_tf: Boolean value to turn transform publisher on/off (Default: true)

How to use

• Connect the sensor breakout (PMW3901 or PAA5100) to the SPI GPIO pins of a Raspberry Pi device:
  • 3-5V to a 3.3V pin
  • CS to BCM 7
  • SCK to BCM 11
  • MOSI to BCM 10
  • MISO to BCM 9
  • INT to BCM 19
  • GND to any ground pin
• Install the pmw3901-python library: sudo pip install pmw3901
• Install the RPi.GPIO library: sudo pip install RPi.GPIO
• Clone this repository in a ROS 2 workspace. Check the sensor_params.yaml file in the config directory, and make any necessary changes.
• Build the package and run the launch file: ros2 launch optical_flow_ros optical_flow_launch.py

Results

This package was tested using a PAA5100JE Near Optical Flow sensor from Pimoroni and ROS 2 Humble on two devices:

• A Raspberry Pi 4 (4GB) running Ubuntu 22.04 with a real-time kernel
• A Raspberry Pi Zero 2 W running Ubuntu 22.04 without any kernel modifications In both cases, the output frequency of 100Hz was achieved.

Challenges

Both these challenges are common for both PMW3901 and PAA5100 boards:

• Secret sauce: the library has a lot of proprietary information which cannot be explained. The manufacturer is apparently very guarded about this information, and the only people who seem to have this information have had to sign an NDA. This includes stuff like the scaling factor to convert from pixel values to distances as well as functionality such as turning the LEDs on and off.

• Off-centered: The sensor breakout from Pimoroni is not centered on the breakout board. Even if this offset is corrected, the aperture of the sensor itself is not centered on the sensor package. Fortunately, Pimoroni has provided a mechanical drawing of the sensor breakout.

optical_flow_ros's Issues

Add functionality to control on-board LEDs

Rotations

The publisher assumes that the sensor is placed at a fixed height parallel to the ground (within the range of the respective sensor), and currently does not account for any rotations about any axis. Rotations in x and y directions (roll and pitch) can be accounted for using the explanation here and in this thesis report referenced in the Bitcraze PMW3901 driver

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 value seems to be proprietary and cannot be found anywhere online, and most references mention an NDA with the sensor manufacturer. This can be calculated by moving the sensor for a known distance and comparing it with the distance measured by the sensor at a fixed height. This still needs to be done once a fixed mount is designed for the sensor. It is also unknown if the scaler value will change for different sensor heights.

Test with PMW3901

This implementation has been tested with the PAA5100 Near Optical Flow sensor but still needs to be tested with the PMW3901 sensor. There may be some differences such as resolution in pixels. Since the used library defines the raw data length to be 1225 (i.e 35x35), a default resolution of 35 is hard-coded in the publisher implementation (similarly the FOV is hard-coded as 42 degrees). However, some online implementations define the resolution of PMW3901 as 30x30. Another issue is the orientation - for PMW3901, this document states that the notch on the sensor indicates the back of the board. However, for PAA5100, the frame seems to be rotated 90 degrees counterclockwise. This is accounted for in the software, for PMW3901, the default orientation is assumed, and for PAA5100, a rotation transformation is applied. Both these issues need to be checked practically with the PMW3901 sensor.

Getting Yaw Information and Angukar.z field?

Firstly, thanks for the great work.

Actually, I am developing something similar using PMW3901 for ground robot. I ahve calibrated the sensor for linear. x and linear.y speeds and verified for known straight distance, seems reasonable. Now, I am looking into how can I extract/generate the yaw information to make a complete twist message and pass on to the EKF.

I have tried doing some vector maths for a full 360 right drive, I find the direction using math.atan2(vy, vx) to get the current theta and then finding the change in theta named as delta_theta. After this I calculate the angualr.z using delta_theta/delta_time. However, when I visualize the data in rviz it seems the robot going towards left while in actual it was a full 360 right drive. and do not represent the curve rather drifts.
I verified with different method using steering feedback with which the robot goes to the right and seems to go in full circle.
Below is method for the same. Could you provide any insight with this. Or is this even the right approach to get the yaw information. The sensor is mounted is same orientation as the The robot (Forward X (+), Left Y (+) ). One more thing the sensor is mounted on the left side of my robot with origin (x = +0.0, y = +0.36, z = +0.150) considering my base_link origin is at centre of the full robot chassis. @adityakamath
`

   def calculate_theta(self, vy,vx):
                                              
    v_magnitude = math.sqrt((vx)**2 + (vy)**2)                                             # Find the magnitude for the velocity
    current_theta = math.atan2(vy,vx)                                                 # Find the current rotating angle of the robot
   
    
    print(f'Current Theta :{current_theta}')
    d_theta = current_theta - self.old_theta                                           # calculate change in rotating  angle
                         
    self.old_theta = current_theta                                                     # Update the rotating angle
    
    
    return  d_theta

`

Finding angular velocity and updating other variables

`

 self.delta_theta = self.calculate_theta(self.vy_ms, self.vx_ms)       
    self.v_angular = self.delta_theta/self.dt                                         # find angular velocity from (delta_theta/dt)
    
    dx = (self.vx_ms*math.cos(self.theta))*self.dt
    dy = (self.vx_ms*math.sin(self.theta))*self.dt
    self.x += dx
    self.y += dy
    self.theta += self.delta_theta

`

colcon build fails [iron]

A fresh checkout in a new workspace on ros2 iron fails to build with the following output:

Starting >>> optical_flow_ros
--- stderr: optical_flow_ros
/usr/lib/python3/dist-packages/setuptools/command/install.py:34: SetuptoolsDeprecationWarning: setup.py install is deprecated. Use build and pip and other standards-based tools.
  warnings.warn(
---
Finished <<< optical_flow_ros [12.2s]

Summary: 1 package finished [22.2s]
  1 package had stderr output: optical_flow_ros