[Closed] RPLidar via GPIO for better performance (Why + Howto + ROS2 Docker)

This took me some time to figure out, I think others might find it useful too…

Background: My ROSBot 2R came with RPLidar A2 wired via the USB-C port on RPi 4B. I’m streaming all ROS data out of the robot (plus some control data in) over WebRTC and have a decent USB 3 wifi adapter. In general this si very fast but I was seeing a lot of lag when the RPLidar was only connected and read from by the driver. There was also a lot of CPU load that seemed unjustified given what the driver is actually doing.

After much research I learned the USB stack on the Pi is generally considered not very good. The dwc2 driver performs poorly, switching to hacky but faster dwc_otg made no apparent improvement in this case. (Btw, Husarion uses dtoverlay=dwc2,dr_mode=host to get the USB-C act as a host and this uses the upstram dwc2 driver. If you want to get the USB-C into host more using dwc_otg, make sure you have no mention of dwc2 in your /boot/config.txt and put otg_mode=1 under [all]; Again, this doesn’t help with the RPLidar problem).

The interrupts on the USB stack were the culprit behind my performance issues. I’ve learned that emulating UART with USB in general doesn’t perform very well for this very reason.

If you have enough GPIO pins available, a much better solution is to ditch the UART-to-USB adapter and wire your RPLidar to the Pi directly. Pi 4B has 6 UART channels, some are inaccessible due to how the Core2 is connected to the Pi on the ROSbot. You’ll need one RX/TX pair and one PWM pin to drive the lidar’s motor. This is how I wired mine:

You’ll also need 5V and ground, I took mine from the CORE2’s hSensor4 that seems to be always on and I’m not using it for anything else:

Then you need to add this to your /boot/config.txt

dtoverlay=uart4 # enables the UART4 channel on pins 8 and 9
dtoverlay=pwm,pin=13,func=4 # sets pin 13 to PWM0

After reboot, you should see pwmchip0 in your /sys/class/pwm and ttyAMA1 in /dev:

$ ls -la /sys/class/pwm
lrwxrwxrwx  1 root gpio 0 Sep 18 01:48 pwmchip0 -> ../../devices/platform/soc/fe20c000.pwm/pwm/pwmchip0

$ ls -la /dev/ | grep ttyAMA
lrwxrwxrwx  1 root root           7 Sep 18 01:48 serial0 -> ttyAMA0
crw-rw----  1 root dialout 204,  64 Sep 18 01:48 ttyAMA0
crw-rw----  1 root dialout 204,  65 Sep 18 01:48 ttyAMA1

The Docker container shipped by Husarion uses sllidar_ros2 from GitHub - Slamtec/sllidar_ros2, this package however does not come with GPIO support for some reason (actually it does but expects some other control circuit in between that I don’t have), so I forked it and implemented this functionality myself. This is how you install it as a Docker container:

$ wget https://raw.githubusercontent.com/PhantomCybernetics/sllidar_ros2/serial_gpio/Dockerfile -O phntm-rplidar.Dockerfile
$ docker build -f phntm-rplidar.Dockerfile -t phntm/rplidar:humble .

This will download and build GitHub - PhantomCybernetics/sllidar_ros2 at serial_gpio, set up ROS2 Humble environment, and build pigpio from source. Pigpio contains mappings for the Pi so this most likely doesn’t work with other boards.

You can then launch the container with a compose config like this one:

version: "3.9"
    image: phntm/rplidar:humble
    container_name: rplidar
    restart: unless-stopped
    privileged: true #GPIO needs this!
      - /dev/ttyAMA1:/dev/ttyAMA1 # this is your UART, not necessary in privileged mode 
    command: >
      ros2 launch sllidar_ros2 sllidar_launch.py
        serial_port:=/dev/ttyAMA1 serial_baudrate:=256000 pwm_pin:=13

… and:

$ docker compose up rplidar -d

Note the serial port is set to /dev/ttyAMA1 (ttyAMA0 talks to Core2), baudrate to 256000 (this is for A2, other RPLidars models may differ) and pwm_pin to 13. All tested on my ROSbot 2R running 64-bit Raspberry Pi OS underneath instead of the default Husarion Ubuntu, but should work there too with no problem.

This package replaces Husarion’s rplidar-docker, generates the same ROS messages into the /scan topic and handles the same services. After launching, you should see messages in /scan and very little CPU time being consumed compared to before. The docker container could be smaller and image build faster, keeping it dev-friendly for now.

Hope this helps!

PS: I discovered a funny bug in the Husarion’s implementation while debugging all this. The Docker health check attempts to read from the /scan topic, but when the motor is not running and no messages being sent, it times out and keeps hanging. If you keep your RPLidar in the default configuration, container running, call the /stop_motor service and wait long enough, these zombie health checks slowly but surely eat up all your memory and the system crashes. I’m not doing any health checks with my docker image (yet), so this issue is also resolved, as well as a few other stability tweaks.


Hi @MirekBurkon,

Thank you very much for the detailed description of the solution. I will inform our hardware team about this solution. The only thing I’m missing is information about the extent to which the processor load has been reduced. Would you have a result on hand showing the result achieved?

It’s a bit hard to give one number that represents the old CPU load, as the USB stack messes up many other things running on the machine. I didn’t write this down exactly before I rewired mine, it had a big impact on my networking as mentioned. Maybe you can compare it to a ROSbot you have. I’m now seeing about 5-10% of one CPU core being used by sllidar in Docker, and much more stable and smoother operation in general.