search

orbbec / OrbbecSDK_ROS2

OrbbecSDK ROS2 wrapper
https://orbbec.github.io/OrbbecSDK_ROS2/
Apache License 2.0
57 stars 19 forks source link

Launching multiple astra mini cameras failing #4

Open huma-meng opened 11 months ago

huma-meng commented 11 months ago

There is the following config file, which is used for both cameras: orbbec_astra_mini.yaml

  /**/**:
    ros__parameters:

      connection_delay: 100

      depth_registration: true
      enable_point_cloud: true
      enable_colored_point_cloud: false
      point_cloud_qos: "default"

      enable_color: true
      color_width: 640
      color_height: 480
      color_fps: 30
      color_format: "RGB"
      flip_color: false
      color_qos: "default"
      color_camera_info_qos: "default"
      enable_color_auto_exposure: true

      enable_depth: true
      depth_width: 640
      depth_height: 480
      depth_fps: 30
      depth_format: "Y11"
      flip_depth: false
      depth_qos: "default"
      depth_camera_info_qos: "default"

      enable_ir: true
      ir_width: 640
      ir_height: 480
      ir_fps: 30
      ir_format: "Y10"
      flip_ir: false
      ir_qos: "default"
      ir_camera_info_qos: "default"
      enable_ir_auto_exposure: true

      publish_tf: true
      tf_publish_rate: 10.0
      enable_publish_extrinsic: false
      enable_d2c_viewer: false
      enable_soft_filter: true

The cameras are started using the following launch file: orbbec_astra_mini_multi.launch.py

  from launch import LaunchDescription
  from launch.actions import DeclareLaunchArgument
  from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, TextSubstitution

  from launch_ros.actions import Node
  from launch_ros.substitutions import FindPackageShare

  def generate_launch_description():
    # GENERAL
    num_of_cams = 2
    # CAMERA 0
    cam_0_name = "slb"
    cam_0_serial_number = "ADA2C1302xx"
    # CAMERA 1
    cam_1_name = "slt"
    cam_1_serial_number = "ADA2C1300xx"

    # Orbbec cleanup node
    node_orbbec_cleanup = Node(
      package="orbbec_camera",
      executable="ob_cleanup_shm_node",
      name="camera",
      output="screen",
    )

    # Orbbec camera node 0
    node_orbbec_astra_mini_0 = Node(
      package="orbbec_camera",
      executable="orbbec_camera_node",
      name=(TextSubstitution(text="camera_"), cam_0_name),
      namespace=(TextSubstitution(text="camera_"), cam_0_name),
      output="screen",
      parameters=[
        PathJoinSubstitution([FindPackageShare("rc_hardware"), "config", "orbbec_astra_mini.yaml"]),
        {"camera_name": cam_0_name},
        {"camera_link_frame_id": (TextSubstitution(text="camera_"), cam_0_name, TextSubstitution(text="_link") )},
        {"serial_number": cam_0_serial_number},
        {"device_num": num_of_cams},
      ],
      remappings=[
        ((TextSubstitution(text="camera_"), cam_0_name, TextSubstitution(text="/depth/color/points")),
          (TextSubstitution(text="camera_"), cam_0_name, TextSubstitution(text="/depth_registered/points"))),
      ],
    )

    # Orbbec camera node 1
    node_orbbec_astra_mini_1 = Node(
      package="orbbec_camera",
      executable="orbbec_camera_node",
      name=(TextSubstitution(text="camera_"), cam_1_name),
      namespace=(TextSubstitution(text="camera_"), cam_1_name),
      output="screen",
      parameters=[
        PathJoinSubstitution([FindPackageShare("rc_hardware"), "config", "orbbec_astra_mini.yaml"]),
        {"camera_name": cam_1_name},
        {"camera_link_frame_id": (TextSubstitution(text="camera_"), cam_1_name, TextSubstitution(text="_link"))},
        {"serial_number": cam_1_serial_number},
        {"device_num": num_of_cams},
      ],
      remappings=[
        ((TextSubstitution(text="camera_"), cam_1_name, TextSubstitution(text="/depth/color/points")),
          (TextSubstitution(text="camera_"), cam_1_name, TextSubstitution(text="/depth_registered/points"))),
      ],
    )

    # TF nodes (just for debugging)
    node_tf = Node(
      package="tf2_ros",
      executable="static_transform_publisher",
      arguments=[
        "0.0",
        "0.0",
        "0.5",
        "0.0",
        "0.0",
        "-1.570796325",
        "map",
        (TextSubstitution(text="camera_"), cam_0_name, TextSubstitution(text="_link"))
      ],
    )

    node_tf_cam = Node(
      package="tf2_ros",
      executable="static_transform_publisher",
      arguments=[
        "0.01168204",
        "0.0",
        "0.04028331",
        "0.0",
        "0.261799",
        "0.0",
        (TextSubstitution(text="camera_"), cam_0_name, TextSubstitution(text="_link")),
        (TextSubstitution(text="camera_"), cam_1_name, TextSubstitution(text="_link"))
      ],
    )

    # Return launch description
    return LaunchDescription([
      # Nodes to launch
      node_orbbec_cleanup,
      node_orbbec_astra_mini_0,
      node_orbbec_astra_mini_1,
      node_tf,
      node_tf_cam,
    ])

After launching, the log looks as follows (only the important logs were copied):

...
[INFO] [ob_cleanup_shm_node-1]: process started with pid [45663]
[INFO] [orbbec_camera_node-2]: process started with pid [45665]
[INFO] [orbbec_camera_node-3]: process started with pid [45667]
...
[ob_cleanup_shm_node-1] [INFO] [1690987337.141217908] [cleanup_shm]: sem_open succeeded
[ob_cleanup_shm_node-1] [INFO] [1690987337.141248065] [cleanup_shm]: sem_unlink succeeded
[ob_cleanup_shm_node-1] [INFO] [1690987337.141257854] [cleanup_shm]: shmctl `IPC_RMID` succeeded
[INFO] [ob_cleanup_shm_node-1]: process has finished cleanly [pid 45663]
...
[orbbec_camera_node-3] WARNING: Logging before InitGoogleLogging() is written to STDERR
[orbbec_camera_node-3] [I20230802 16:42:17.248329 45667 Context.cpp:13] Context creating!
[orbbec_camera_node-3] [I20230802 16:42:17.249301 45667 XmlConfig.cpp:108] loadConfigFile() using defaultConfig_==/home/user/ros2_humble/extern_ws/install/orbbec_camera/share/orbbec_camera/config/OrbbecSDKConfig_v1.0.xml
[orbbec_camera_node-3] [I20230802 16:42:17.249503 45667 Context.cpp:33] Config file version=1.1
[orbbec_camera_node-3] [I20230802 16:42:17.249701 45667 FrameBufferManager.cpp:22] Max global frame buffer size updated! Size=2048MB
[orbbec_camera_node-3] [I20230802 16:42:17.249923 45667 Context.cpp:78] filter version[major.minor.maintenance.build]: 1.1.4.0
[orbbec_camera_node-3] [I20230802 16:42:17.250128 45667 DeviceManager.cpp:54] DeviceManager init ...
[orbbec_camera_node-3] [I20230802 16:42:17.250794 45667 LinuxPal.cpp:21] createObPal: create LinuxPal!
[orbbec_camera_node-2] WARNING: Logging before InitGoogleLogging() is written to STDERR
[orbbec_camera_node-2] [I20230802 16:42:17.258785 45665 Context.cpp:13] Context creating!
[orbbec_camera_node-2] [I20230802 16:42:17.259483 45665 XmlConfig.cpp:108] loadConfigFile() using defaultConfig_==/home/user/ros2_humble/extern_ws/install/orbbec_camera/share/orbbec_camera/config/OrbbecSDKConfig_v1.0.xml
[orbbec_camera_node-2] [I20230802 16:42:17.259637 45665 Context.cpp:33] Config file version=1.1
[orbbec_camera_node-2] [I20230802 16:42:17.259780 45665 FrameBufferManager.cpp:22] Max global frame buffer size updated! Size=2048MB
[orbbec_camera_node-2] [I20230802 16:42:17.259915 45665 Context.cpp:78] filter version[major.minor.maintenance.build]: 1.1.4.0
[orbbec_camera_node-2] [I20230802 16:42:17.260056 45665 DeviceManager.cpp:54] DeviceManager init ...
[orbbec_camera_node-2] [I20230802 16:42:17.260255 45665 LinuxPal.cpp:21] createObPal: create LinuxPal!
[orbbec_camera_node-3] [I20230802 16:42:17.264209 45667 EnumeratorLibusb.cpp:325] queryDevicesInfo done!
[orbbec_camera_node-3] [E20230802 16:42:17.264230 45667 ObV4lDevice.cpp:330] Failed to open /sys/class/video4linux
[orbbec_camera_node-3] [I20230802 16:42:17.264235 45667 DeviceManager.cpp:373] Current usb device port list::
[orbbec_camera_node-3] [I20230802 16:42:17.264238 45667 DeviceManager.cpp:375]         - 3-2.2-18.0 | 
[orbbec_camera_node-3] [I20230802 16:42:17.264240 45667 DeviceManager.cpp:375]         - 1-2-17.0 | Orbbec(R) Astra(TM) 3D Camera(F) Depth
[orbbec_camera_node-3] [W20230802 16:42:17.264247 45667 OpenNIDeviceInfo.cpp:110] New openni device matched.
[orbbec_camera_node-3] [W20230802 16:42:17.264253 45667 OpenNIDeviceInfo.cpp:110] New openni device matched.
[orbbec_camera_node-3] [I20230802 16:42:17.264257 45667 DeviceManager.cpp:345] Devices matched:
[orbbec_camera_node-3] [I20230802 16:42:17.264259 45667 DeviceManager.cpp:359]   - openniDevice = 2
[orbbec_camera_node-3] [I20230802 16:42:17.264313 45667 LinuxPal.cpp:142] Create PollingDeviceWatcher!
[orbbec_camera_node-3] [I20230802 16:42:17.264318 45667 DeviceManager.cpp:99] DeviceManager init  done!
[orbbec_camera_node-3] [I20230802 16:42:17.264320 45667 DeviceManager.cpp:50] DeviceManager construct done
[orbbec_camera_node-3] [I20230802 16:42:17.264322 45667 Context.cpp:51] Context created!
[orbbec_camera_node-3] [INFO] [1690987337.264538032] [camera.camera_slt]: Waiting for device connection...
[orbbec_camera_node-3] [INFO] [1690987337.264627983] [camera.camera_slt]: onDeviceConnected
[orbbec_camera_node-3] [INFO] [1690987337.264664442] [camera.camera_slt]: Connecting to device with serial number: ADA2C1302xx
[orbbec_camera_node-3] [INFO] [1690987337.264675624] [camera.camera_slt]: semaphore value: 1
[orbbec_camera_node-2] [I20230802 16:42:17.277216 45665 EnumeratorLibusb.cpp:325] queryDevicesInfo done!
[orbbec_camera_node-2] [E20230802 16:42:17.277237 45665 ObV4lDevice.cpp:330] Failed to open /sys/class/video4linux
[orbbec_camera_node-2] [I20230802 16:42:17.277243 45665 DeviceManager.cpp:373] Current usb device port list::
[orbbec_camera_node-2] [I20230802 16:42:17.277246 45665 DeviceManager.cpp:375]         - 3-2.2-18.0 | 
[orbbec_camera_node-2] [I20230802 16:42:17.277248 45665 DeviceManager.cpp:375]         - 1-2-17.0 | Orbbec(R) Astra(TM) 3D Camera(F) Depth
[orbbec_camera_node-2] [W20230802 16:42:17.277253 45665 OpenNIDeviceInfo.cpp:110] New openni device matched.
[orbbec_camera_node-2] [W20230802 16:42:17.277259 45665 OpenNIDeviceInfo.cpp:110] New openni device matched.
[orbbec_camera_node-2] [I20230802 16:42:17.277263 45665 DeviceManager.cpp:345] Devices matched:
[orbbec_camera_node-2] [I20230802 16:42:17.277266 45665 DeviceManager.cpp:359]   - openniDevice = 2
[orbbec_camera_node-2] [I20230802 16:42:17.277318 45665 LinuxPal.cpp:142] Create PollingDeviceWatcher!
[orbbec_camera_node-2] [I20230802 16:42:17.277323 45665 DeviceManager.cpp:99] DeviceManager init  done!
[orbbec_camera_node-2] [I20230802 16:42:17.277325 45665 DeviceManager.cpp:50] DeviceManager construct done
[orbbec_camera_node-2] [I20230802 16:42:17.277328 45665 Context.cpp:51] Context created!
[orbbec_camera_node-2] [INFO] [1690987337.277506670] [camera.camera_slb]: Waiting for device connection...
[orbbec_camera_node-2] [INFO] [1690987337.277602312] [camera.camera_slb]: onDeviceConnected
[orbbec_camera_node-2] [INFO] [1690987337.277622931] [camera.camera_slb]: Connecting to device with serial number: ADA2C1300xx
[orbbec_camera_node-2] [INFO] [1690987337.277632058] [camera.camera_slb]: semaphore value: 0
[orbbec_camera_node-3] [ERROR] [1690987338.264543973] [camera.camera_slt]: checkConnectTimer: device ADA2C1302xx not connected
[orbbec_camera_node-2] [ERROR] [1690987338.277501610] [camera.camera_slb]: checkConnectTimer: device ADA2C1300xx not connected
[orbbec_camera_node-3] [ERROR] [1690987339.264559497] [camera.camera_slt]: checkConnectTimer: device ADA2C1302xx not connected
[orbbec_camera_node-2] [ERROR] [1690987339.277518836] [camera.camera_slb]: checkConnectTimer: device ADA2C1300xx not connected
[orbbec_camera_node-3] [ERROR] [1690987340.264585776] [camera.camera_slt]: checkConnectTimer: device ADA2C1302xx not connected
[orbbec_camera_node-2] [ERROR] [1690987340.277542731] [camera.camera_slb]: checkConnectTimer: device ADA2C1300xx not connected

Until (most of the time) the following error occurs:

[orbbec_camera_node-3] [ERROR] [1690989609.841805670] [camera_slt.camera_slt]: checkConnectTimer: device ADA2C1302xx not connected
[orbbec_camera_node-3] [ERROR] [1690989610.622409726] [camera_slt.camera_slt]: Failed to start pipeline: Start stream failed, OB_SENSOR_DEPTH config video mode failed!
[orbbec_camera_node-3] [INFO] [1690989610.622447508] [camera_slt.camera_slt]: try to disable ir stream and try again
[orbbec_camera_node-3] [INFO] [1690989610.622461715] [camera_slt.camera_slt]: Enable depth stream
[orbbec_camera_node-3] [INFO] [1690989610.622469359] [camera_slt.camera_slt]: Stream depth width: 640 height: 480 fps: 30 format: Y11
[orbbec_camera_node-3] [INFO] [1690989610.622475421] [camera_slt.camera_slt]: Enable color stream
[orbbec_camera_node-3] [INFO] [1690989610.622480240] [camera_slt.camera_slt]: Stream color width: 640 height: 480 fps: 30 format: RGB
[orbbec_camera_node-3] terminate called without an active exception
[orbbec_camera_node-3] *** Aborted at 1690989610 (unix time) try "date -d @1690989610" if you are using GNU date ***
[orbbec_camera_node-3] PC: @                0x0 (unknown)
[orbbec_camera_node-3] *** SIGABRT (@0x3e800012b12) received by PID 76562 (TID 0x7f0172ffd640) from PID 76562; stack trace: ***
[orbbec_camera_node-3]     @     0x7f01fb442520 (unknown)
[orbbec_camera_node-3]     @     0x7f01fb496a7c pthread_kill
[orbbec_camera_node-3]     @     0x7f01fb442476 raise
[orbbec_camera_node-3]     @     0x7f01fb4287f3 abort
[orbbec_camera_node-3]     @     0x7f01fb8a2b9e (unknown)
[orbbec_camera_node-3]     @     0x7f01fb8ae20c (unknown)
[orbbec_camera_node-3]     @     0x7f01fb8ae277 std::terminate()
[orbbec_camera_node-3]     @     0x7f01fbe6bae2 std::_Sp_counted_ptr_inplace<>::_M_dispose()
[orbbec_camera_node-3]     @     0x5641d5b0e6fa std::_Sp_counted_base<>::_M_release()
[orbbec_camera_node-3]     @     0x7f01fbe10150 _ZN13orbbec_camera12OBCameraNodeC2EPN6rclcpp4NodeESt10shared_ptrIN2ob6DeviceEES4_INS_10ParametersEE.cold
[orbbec_camera_node-3]     @     0x7f01fbe6455b orbbec_camera::OBCameraNodeDriver::initializeDevice()
[orbbec_camera_node-3]     @     0x7f01fbe6a89f orbbec_camera::OBCameraNodeDriver::startDevice()
[orbbec_camera_node-3]     @     0x7f01fbe6afbc orbbec_camera::OBCameraNodeDriver::onDeviceConnected()
[orbbec_camera_node-3]     @     0x7f01fbe6b6df orbbec_camera::OBCameraNodeDriver::queryDevice()
[orbbec_camera_node-3]     @     0x7f01fb8dc253 (unknown)
[orbbec_camera_node-3]     @     0x7f01fb494b43 (unknown)
[orbbec_camera_node-3]     @     0x7f01fb526a00 (unknown)
[ERROR] [orbbec_camera_node-3]: process has died [pid 76562, exit code -6, cmd '/home/maurice/ros2_humble/extern_ws/install/orbbec_camera/lib/orbbec_camera/orbbec_camera_node --ros-args -r __node:=camera_slt -r __ns:=/camera_slt --params-file /home/maurice/ros2_humble/dev_ws/install/thi_rc_hardware/share/thi_rc_hardware/config/orbbec_astra_mini.yaml --params-file /tmp/launch_params_tnshbb4y --params-file /tmp/launch_params_4d44tqc7 --params-file /tmp/launch_params_bnw990lp --params-file /tmp/launch_params_hlyorvhw -r camera_slt/depth/color/points:=camera_slt/depth_registered/points'].

After terminating the nodes with Ctrl+C, mostly one of the cameras is no longer displayed with its serial number. Sometimes, both cameras are no longer detected. ros2 run orbbec_camera list_devices_node

WARNING: Logging before InitGoogleLogging() is written to STDERR
[I20230802 17:21:42.436887 76796 Context.cpp:13] Context creating!
[I20230802 17:21:42.436976 76796 Context.cpp:78] filter version[major.minor.maintenance.build]: 1.1.4.0
[I20230802 17:21:42.436990 76796 DeviceManager.cpp:54] DeviceManager init ...
[I20230802 17:21:42.437074 76796 LinuxPal.cpp:21] createObPal: create LinuxPal!
[I20230802 17:21:42.448864 76796 EnumeratorLibusb.cpp:325] queryDevicesInfo done!
[E20230802 17:21:42.448887 76796 ObV4lDevice.cpp:330] Failed to open /sys/class/video4linux
[I20230802 17:21:42.448899 76796 DeviceManager.cpp:373] Current usb device port list::
[I20230802 17:21:42.448904 76796 DeviceManager.cpp:375]         - 3-2.2-22.0 | 
[I20230802 17:21:42.448910 76796 DeviceManager.cpp:375]         - 1-2-21.0 | Orbbec(R) Astra(TM) 3D Camera(F) Depth
[W20230802 17:21:42.448920 76796 OpenNIDeviceInfo.cpp:110] New openni device matched.
[W20230802 17:21:42.448931 76796 OpenNIDeviceInfo.cpp:110] New openni device matched.
[I20230802 17:21:42.448937 76796 DeviceManager.cpp:345] Devices matched:
[I20230802 17:21:42.448940 76796 DeviceManager.cpp:359]   - openniDevice = 2
[I20230802 17:21:42.448982 76796 LinuxPal.cpp:142] Create PollingDeviceWatcher!
[I20230802 17:21:42.448988 76796 DeviceManager.cpp:99] DeviceManager init  done!
[I20230802 17:21:42.448993 76796 DeviceManager.cpp:50] DeviceManager construct done
[I20230802 17:21:42.448999 76796 Context.cpp:51] Context created!
[INFO] [1690989702.449613114] [list_device_node]: serial: 
[INFO] [1690989702.792987553] [list_device_node]: serial: ADA2C1300xx

The cameras must then be unplugged and plugged back in to be detected again. The scenario where both cameras are working is almost never the case.