• HardwareROS
  • depthai-ros Pointcloud with "no IMU" Oak-D-Lite?

How am I going to be able to publish depthai-ros pointcloud2 messages with a "no IMU" Oak-D-Lite camera?

I've confirmed I can successfully run:

  • depthai-ros camera, (RGB video and MobileNet NN detections)
    changed camera.yaml for "no IMU":
    depthai-ros/depthai_ros_driver/config $ diff camera.yaml camera.yaml.orig
    4c4
    <       i_enable_imu: false
    ---
    >       i_enable_imu: true
    
    ros2 launch depthai_ros_driver camera.launch.py
    
    cmds/echo_oak_nn_spatial_detections.sh 
    
    ** detections: 
    class_id='9', score=0.63134765625
    class_id='11', score=0.509765625
  • yolo4 publisher (RGB video and yolo detections as ROS 2 Humble topics)

    ros2 launch depthai_examples yolov4_publisher.launch.py camera_model:=OAK-D-LITE spatial_camera:=false 
  • mobilenet publisher node: (RGB image and mobilenet detections)

    ros2 launch depthai_examples mobile_publisher.launch.py camera_model:=OAK-D-LITE

but no success with (all fail with no IMU error):

  • rgbd.pcl.launch.py

    ros2 launch depthai_ros_driver rgbd.pcl.launch.py
    ...
    [component_container-1] [ERROR] [1706942642.296201358] [oak_container]: Component constructor threw an exception: IMU(9) - IMU invalid settings!: IMU not detected. Your board doesn't have IMU or requires firmware update (Set imu.enableFirmwareUpdate(True) explicitly.). 0, 328
    [ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'oak' of type 'depthai_ros_driver::Camera' in container '/oak_container': Component constructor threw an exception: IMU(9) - IMU invalid settings!: IMU not detected. Your board doesn't have IMU or requires firmware update (Set imu.enableFirmwareUpdate(True) explicitly.). 0, 328
  • example_segmentation.launch.py

    ros2 launch depthai_ros_driver example_segmentation.launch.py
    ...
    [ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'oak' of type 'depthai_ros_driver::Camera' in container '/oak_container': Component constructor threw an exception: IMU(7) - IMU invalid settings!: IMU not detected. Your board doesn't have IMU
  • pointcloud.launch.py

    ros2 launch depthai_ros_driver pointcloud.launch.py
    ...
    [component_container-1] [ERROR] [1706942958.672348873] [oak_container]: Component constructor threw an exception: IMU(10) - IMU invalid settings!: IMU not detected. Your board doesn't have IMU or requires firmware update (Set imu.enableFirmwareUpdate(True) explicitly.). 0, 328
    [ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'oak' of type 'depthai_ros_driver::Camera' in container '/oak_container': Component constructor threw an exception: IMU(10) - IMU invalid settings!: IMU not detected. Your board doesn't have IMU
  • rtabmap.launch.py
    ros2 launch depthai_ros_driver rtabmap.launch.py
    ...
    [component_container-1] [ERROR] [1706942114.645790128] [oak_container]: Component constructor threw an exception: IMU(9) - IMU invalid settings!: IMU not detected. Your board doesn't have IMU or requires firmware update (Set imu.enableFirmwareUpdate(True) explicitly.). 0, 328
    [ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'oak' of type 'depthai_ros_driver::Camera' in container '/oak_container': Component constructor threw an exception: IMU(9) - IMU invalid settings!: IMU not detected. Your board doesn't have IMU or requires firmware update (Set imu.enableFirmwareUpdate(True) explicitly.). 0, 328
    ...

and cannot use the basic oak camera to roll my own:

mre.py:  (stripped down to minimum reproducible error from the non-ROS Pointcloud example)
from depthai_sdk import OakCamera

with OakCamera() as oak:
    color = oak.camera('color')


$ python3 mre.py
Closing OAK camera
Traceback (most recent call last):
  File "/home/pi/wali_pi5/dai_ws/mre.py", line 4, in <module>
    color = oak.camera('color')
AttributeError: 'OakCamera' object has no attribute 'camera'

and if I change to oak.create_camera('color') it the pointcloud demo fails with:

$ python3 pointcloud.py
['__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__enter__', '__eq__', '__exit__', '__format__', '__ge__', '__getattribute__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_callback', '_init_device', 'build', 'callback', 'config_camera', 'config_pipeline', 'create_camera', 'create_imu', 'create_nn', 'create_stereo', 'device', 'poll', 'record', 'running', 'sensors', 'show_graph', 'start', 'sync', 'visualize']
Closing OAK camera
Traceback (most recent call last):
  File "/home/pi/wali_pi5/systests/Oak-D-Lite/vdepthai/depthai/depthai_sdk/examples/PointcloudComponent/pointcloud.py", line 9, in <module>
    pcl = oak.create_pointcloud(stereo=stereo, colorize=color)
AttributeError: 'OakCamera' object has no attribute 'create_pointcloud'

UPDATE:

In my environment the installed oak_camera.py
lib/python3.11/site-packages/depthai_sdk/oak_camera.py
does not include the create_pointcloud class method.

This file does: depthai/depthai_sdk/src/depthai_sdk/oak_camera.py
but that file expects ahrs.filters in xout_imu.py which also is missing in the installed site-packages.

Seeing this leads me to believe that even if the create_pointcloud() method existed, the lack of an IMU in my particular Oak-D-Lite would prevent me from using the method.

Someone with a TurtleBot3 mentioned using a ROS package that subscribes to DepthClouds to output PointClouds - guessing that is the route I need to investigate.

  • jakaskerl replied to this.
    • Best Answerset by cycob

    Oh WOW - I got rtabmap sort of working...with the params_file!

    $ more no_imu.yaml 
    /oak:
      ros__parameters:
        camera:
          i_enable_imu: false
          i_enable_ir: false
    
    
    
    $ ros2 launch depthai_ros_driver rtabmap.launch.py params_file:=/home/pi/wali_pi5/dai_ws/no_imu.yaml

    And it does not seem to be taxing the Raspberry Pi 5 very much:

    ********** WaLiPi5  MONITOR ******************************
    Monday 02/05/24 12:10:47 up 7 days, 19:28,  5 users,  load average: 1.62, 0.97, 1.08
    temp=67.5'C
    frequency(0)=2400008192
    throttled=0x0
                   total        used        free      shared  buff/cache   available
    Mem:           7.9Gi       2.0Gi       630Mi        62Mi       5.4Gi       5.9Gi
    Swap:           99Mi        99Mi       960Ki
    CONTAINER ID   IMAGE     COMMAND                  CREATED        STATUS        PORTS     NAMES
    51debb7f004f   r2hdp     "/ros_entrypoint.sh …"   36 hours ago   Up 36 hours             r2hdp
    
    
    285410 /usr/bin/python3 /opt/ros/humble/bin/ros2 launch teleop_twist_joy teleop-launch.py joy_config:=F710
    285411 /usr/bin/python3 /opt/ros/humble/bin/ros2 run wali odometer
    285412 /usr/bin/python3 /opt/ros/humble/bin/ros2 run wali say_node
    285413 /usr/bin/python3 /opt/ros/humble/bin/ros2 run wali ir2scan
    285414 /usr/bin/python3 /opt/ros/humble/bin/ros2 run wali wali_node
    302367 /usr/bin/python3 /opt/ros/humble/bin/ros2 launch depthai_ros_driver rtabmap.launch.py params_file:=/home/pi/wali_pi5/dai_ws/no_imu.yaml

    Thank you!

    Hi cycob
    Which depthai_sdk version are you using? Afaik, IMU is not a requirement for pointcloud generation (at least not on the SDK). Not sure about ROS; cc @Luxonis-Adam

    Thanks,
    Jaka

      jakaskerl Which depthai_sdk version are you using?

      Relevant output from log_system_information.py

              "depthai==2.21.2.0",
              "depthai-ros-driver==2.8.2",
              "depthai-ros-msgs==2.8.2",
              "depthai-sdk==1.9.4",

      @jakaskerl @Luxonis-Adam

      I cannot find any way to launch the pointcloud node with my "no-IMU" Oak-D-Lite camera:
      (tried with/without param i_enable_imu:=false)

      $ ./launch_pointcloud.sh 
      [INFO] [launch]: All log files can be found below /home/pi/wali_pi5/c3ws/roslogs/2024-02-04-22-50-29-179351-WaLiPi5-2073
      [INFO] [launch]: Default logging verbosity is set to INFO
      [INFO] [component_container-1]: process started with pid [2088]
      [component_container-1] [INFO] [1707105029.754148776] [oak_container]: Load Library: /opt/ros/humble/lib/librobot_state_publisher_node.so
      [component_container-1] [INFO] [1707105029.758434632] [oak_container]: Found class: rclcpp_components::NodeFactoryTemplate<robot_state_publisher::RobotStatePublisher>
      [component_container-1] [INFO] [1707105029.759005911] [oak_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<robot_state_publisher::RobotStatePublisher>
      [component_container-1] [INFO] [1707105029.776188764] [oak_state_publisher]: got segment oak
      [component_container-1] [INFO] [1707105029.776246523] [oak_state_publisher]: got segment oak-d-base-frame
      [component_container-1] [INFO] [1707105029.776260542] [oak_state_publisher]: got segment oak_imu_frame
      [component_container-1] [INFO] [1707105029.776272023] [oak_state_publisher]: got segment oak_left_camera_frame
      [component_container-1] [INFO] [1707105029.776284338] [oak_state_publisher]: got segment oak_left_camera_optical_frame
      [component_container-1] [INFO] [1707105029.776295764] [oak_state_publisher]: got segment oak_model_origin
      [component_container-1] [INFO] [1707105029.776305894] [oak_state_publisher]: got segment oak_rgb_camera_frame
      [component_container-1] [INFO] [1707105029.776317227] [oak_state_publisher]: got segment oak_rgb_camera_optical_frame
      [component_container-1] [INFO] [1707105029.776328412] [oak_state_publisher]: got segment oak_right_camera_frame
      [component_container-1] [INFO] [1707105029.776339653] [oak_state_publisher]: got segment oak_right_camera_optical_frame
      [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/oak_state_publisher' in container 'oak_container'
      [component_container-1] [INFO] [1707105029.782189548] [oak_container]: Load Library: /home/pi/wali_pi5/dai_ws/install/depthai_ros_driver/lib/libdepthai_ros_driver.so
      [component_container-1] [INFO] [1707105029.875179007] [oak_container]: Found class: rclcpp_components::NodeFactoryTemplate<depthai_ros_driver::Camera>
      [component_container-1] [INFO] [1707105029.875275858] [oak_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<depthai_ros_driver::Camera>
      [component_container-1] [INFO] [1707105029.896919717] [oak]: No ip/mxid specified, connecting to the next available device.
      [component_container-1] [INFO] [1707105032.130643479] [oak]: Camera with MXID: 184430101175A41200 and Name: 3.1 connected!
      [component_container-1] [INFO] [1707105032.131766628] [oak]: USB SPEED: SUPER
      [component_container-1] [INFO] [1707105032.153159430] [oak]: Device type: OAK-D-LITE
      [component_container-1] [INFO] [1707105032.155825822] [oak]: Pipeline type: RGBD
      [component_container-1] [INFO] [1707105032.809933859] [oak]: Finished setting up pipeline.
      [component_container-1] [ERROR] [1707105035.135015371] [oak_container]: Component constructor threw an exception: IMU(10) - IMU invalid settings!: IMU not detected. Your board doesn't have IMU or requires firmware update (Set imu.enableFirmwareUpdate(True) explicitly.). 0, 328
      [component_container-1] [INFO] [1707105035.136224705] [oak_container]: Load Library: /opt/ros/humble/lib/libdepth_image_proc.so
      [ERROR] [launch_ros.actions.load_composable_nodes]: Failed to load node 'oak' of type 'depthai_ros_driver::Camera' in container '/oak_container': Component constructor threw an exception: IMU(10) - IMU invalid settings!: IMU not detected. Your board doesn't have IMU or requires firmware update (Set imu.enableFirmwareUpdate(True) explicitly.). 0, 328
      [component_container-1] [INFO] [1707105035.142136805] [oak_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::ConvertMetricNode>
      [component_container-1] [INFO] [1707105035.142215842] [oak_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::CropForemostNode>
      [component_container-1] [INFO] [1707105035.142231916] [oak_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::DisparityNode>
      [component_container-1] [INFO] [1707105035.142246360] [oak_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::PointCloudXyzNode>
      [component_container-1] [INFO] [1707105035.142260342] [oak_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::PointCloudXyzRadialNode>
      [component_container-1] [INFO] [1707105035.142275027] [oak_container]: Found class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::PointCloudXyziNode>
      [component_container-1] [INFO] [1707105035.142289027] [oak_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<depth_image_proc::PointCloudXyziNode>
      [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/point_cloud_xyzi' in container 'oak_container'

      NO POINTCLOUD WITHOUT AN IMU IN THE CAMERA with depthai-ros

      Hi,
      could you try upgrading depthai library sudo apt install ros-humble-depthai? The driver should still run even if IMU is not detected as it checks for it using firmware library, if that's not the case you can still edit config files for other launch files (they are located in config directory in depthai_ros_driver directory, you can also pass external config file using launch file arguments) to disable IMU explicitly.

        Luxonis-Adam could you try upgrading depthai library sudo apt install ros-humble-depthai?

        pi@WaLiPi5:~/wali_pi5/dai_ws $ sudo apt update && sudo apt install ros-humble-depthai
        ...
        Reading package lists... Done
        Building dependency tree... Done
        Reading state information... Done
        ros-humble-depthai is already the newest version (2.23.0-1jammy.20240125.203547).

        Luxonis-Adam The driver should still run even if IMU is not detected as it checks for it using firmware library,

        Yes, much of the pointcloud.launch.py composite ROS node is alive but the oak camera exceptions when not finding the IMU. The camera is failing due to oak.create_pointcloud() (the C++ equivalent somewhere) failing.

        [oak_container]: Component constructor threw an exception: IMU(10) - IMU invalid settings!: IMU not detected. Your board doesn't have IMU 

        Luxonis-Adam you can still edit config files for other launch files (they are located in config directory in depthai_ros_driver directory

        Yes, I have modified: dai_ws/src/depthai-ros/depthai_ros_driver/config/camera.yaml,
        which build copies to: dai_ws/install/depthai_ros_driver/share/depthai_ros_driver/config/camera.yaml -

        camera.yaml: 
        /oak:
          ros__parameters:
            camera:
              i_enable_imu: false
              i_enable_ir: true
              i_nn_type: spatial
              i_pipeline_type: RGBD
            nn:
              i_nn_config_path: depthai_ros_driver/mobilenet

        but it does not appear to affect the pointcloud.launch.py composite camera node.
        (that param file also configures a MobileNet NN so additionally appears to not be the correct param file).

        Luxonis-Adam you can also pass external config file using launch file arguments) to disable IMU explicitly.

        I created no_imu.yaml:

        /oak:
          ros__parameters:
            camera:
              i_enable_imu: false
              i_enable_ir: false

        and using command:

        ros2 launch depthai_ros_driver pointcloud.launch.py params_file:=/home/pi/wali_pi5/dai_ws/no_imu.yaml

        The camera node appears to be launching without exception:

        [component_container-1] [INFO] [1707147242.231740145] [oak]: Camera with MXID: 184430101175A41200 and Name: 3.1 connected!
        [component_container-1] [INFO] [1707147242.238504726] [oak]: USB SPEED: SUPER
        [component_container-1] [INFO] [1707147242.260400946] [oak]: Device type: OAK-D-LITE
        [component_container-1] [INFO] [1707147242.263645533] [oak]: Pipeline type: RGBD
        [component_container-1] [INFO] [1707147242.389490866] [oak]: NN Family: mobilenet
        [component_container-1] [INFO] [1707147242.485281992] [oak]: NN input size: 300 x 300. Resizing input image in case of different dimensions.
        [component_container-1] [INFO] [1707147242.568013457] [oak]: Finished setting up pipeline.
        [component_container-1] [INFO] [1707147243.187124674] [oak]: Camera ready!
        [INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/oak' in container '/oak_container'

        The camera is running mobilenet and publishing /oak/detections (is that ok? Love it if so)

        The /point_cloud_xyzi node is running but does not publish anything on /oak/points (0 msgs)

        ros2 node info /point_cloud_xyzi 
        /point_cloud_xyzi
          Subscribers:
            /oak/right/image_rect: sensor_msgs/msg/Image
            /oak/stereo/camera_info: sensor_msgs/msg/CameraInfo
            /oak/stereo/image_raw: sensor_msgs/msg/Image
            /parameter_events: rcl_interfaces/msg/ParameterEvent
          Publishers:
            /oak/points: sensor_msgs/msg/PointCloud2
            /parameter_events: rcl_interfaces/msg/ParameterEvent
            /rosout: rcl_interfaces/msg/Log
        
        $ ros2 topic hz /oak/points
        WARNING: topic [/oak/points] does not appear to be published yet

        I confirmed the camera is outputting /oak/stereo/image_raw, which appears to be a depth image.

        Oh WOW - I got rtabmap sort of working...with the params_file!

        $ more no_imu.yaml 
        /oak:
          ros__parameters:
            camera:
              i_enable_imu: false
              i_enable_ir: false
        
        
        
        $ ros2 launch depthai_ros_driver rtabmap.launch.py params_file:=/home/pi/wali_pi5/dai_ws/no_imu.yaml

        And it does not seem to be taxing the Raspberry Pi 5 very much:

        ********** WaLiPi5  MONITOR ******************************
        Monday 02/05/24 12:10:47 up 7 days, 19:28,  5 users,  load average: 1.62, 0.97, 1.08
        temp=67.5'C
        frequency(0)=2400008192
        throttled=0x0
                       total        used        free      shared  buff/cache   available
        Mem:           7.9Gi       2.0Gi       630Mi        62Mi       5.4Gi       5.9Gi
        Swap:           99Mi        99Mi       960Ki
        CONTAINER ID   IMAGE     COMMAND                  CREATED        STATUS        PORTS     NAMES
        51debb7f004f   r2hdp     "/ros_entrypoint.sh …"   36 hours ago   Up 36 hours             r2hdp
        
        
        285410 /usr/bin/python3 /opt/ros/humble/bin/ros2 launch teleop_twist_joy teleop-launch.py joy_config:=F710
        285411 /usr/bin/python3 /opt/ros/humble/bin/ros2 run wali odometer
        285412 /usr/bin/python3 /opt/ros/humble/bin/ros2 run wali say_node
        285413 /usr/bin/python3 /opt/ros/humble/bin/ros2 run wali ir2scan
        285414 /usr/bin/python3 /opt/ros/humble/bin/ros2 run wali wali_node
        302367 /usr/bin/python3 /opt/ros/humble/bin/ros2 launch depthai_ros_driver rtabmap.launch.py params_file:=/home/pi/wali_pi5/dai_ws/no_imu.yaml

        Thank you!