Enhanced Object Avoidance in SLAM Using PMD USB Flexx2 Camera


This application note describes a process of employing PMD Flexx2 ToF camera to improve object avoidance during iRobot Create3 navigation.

The enhanced obstacle avoidance is achieved by factoring in PMD Flexx2 sensor telemetry which ‘sees’ smaller objects in front of iRobot Create3 lying ahead of the robot below the LIDAR’s scanning plane.

The application note assumes that the necessary hardware and software setup has been performed.

Step 1. Launch ROS2 Navigation Stack

Proceed with https://voxelbotics.atlassian.net/wiki/spaces/DOC/pages/43745427 instructions to launch the ROS2 navigation stack with the enabled ToF camera node, and launch Rviz in the default (3D) mode.

Step 2. Camera Position Calibration

This step is needed to ensure that the camera configuration corresponds to the actual positioning and its telemetry matches the telemetry from the LIDAR.

  1. Put a box in front of the robot.

  2. In the RVIZ window check that the LIDAR scan points match the box’s front side.

    The LIDAR Red Line Matches the Visible Box Side
  3. If the red scan points from LIDAR which demarcate the front side of the box do not match the box’s front side projection from the camera, adjust the camera’s angle and position such that the matching is achieved.

Step 3. (Optional) Adjust the ToF Camera Configuration

This step is only required if the calibration step doesn’t provide satisfying matching between the LIDAR scan and the Camera point clouds.

  1. Activate the Terminal window where the start ROS2 navigation stack command has been executed.

  2. Stop the navigation stack (press Control-C).

  3. Launch the vi text editor with the PMD camera configuration file.
    NOTE: Use the “user" password when prompted:

    sudo vi /opt/ros/humble/share/create3_slam_launch/config/pmd_camera_params.yaml


    pmd_camera_node: ros__parameters: camera_id: '' min_distance: 0.35 max_distance: 4.0 min_confidence: 0.9 use_case: '' pmd_camera_transform: ros__parameters: child_frame_id: pmd_optical_frame frame_id: base_footprint rotation: w: 1.0 x: 0.0 y: 0.0 z: 0.0 translation: x: -0.1 y: 0.0 z: 0.17 ~
  4. Press i to enter the vi's insert mode and replace the translation section parameters ‘X,Y,Z’ according to the actual ToF camera placement.
    X - forward (plus)/back(minus) offset relative to the robot’s center, in meters;
    Y - left (plus) / right (minus) offset relative to the robot’s center, in meters;
    Z - up (plus) offset relative to the robot’s footprint (bottom), in meters.
    The value tolerance is +/- 2cm.

  5. Once updated, press ESC, then SHIFT + ':'. Type wq in the prompt and then press ENTER.

  6. Restart the ROS2 navigation stack and perform the Calibration step.

  7. Repeat steps 1 - 6 until the calibration results are satisfactory.

Step 4. Test Enhanced Obstacle Avoidance

To test if the PMD camera adds up to the obstacle avoidance we suggest using small objects like a cup.

  1. Put the cup on the floor ahead of the robot.

  2. Use Rviz to send a navigation goal such that the straight trajectory to the goal went across the cup.

  3. Verify that the robot does attempt to avoid collision with the object and corrects the trajectory around the cup.

Related Topics