search

orbbec / OrbbecSDK_ROS2

OrbbecSDK ROS2 wrapper
Apache License 2.0
92 stars 36 forks source link

Publishing static transforms from camera_link provokes NaN values for Femto Bolt #46

Open Danilrivero opened 5 months ago

Danilrivero commented 5 months ago

General information

Explanation

I use the camera for certain detections, all of these are relative to the _cameraframe, which need to be transformed to another frame by using tf2. Launching the driver with the following command:

ros2 launch orbbec_camera femto_bolt.launch.py     connection_delay:=500     enable_point_cloud:=true     enable_colored_point_cloud:=true     depth_registration:=true     color_width:=1280     color_height:=960     color_fps:=30     depth_width:=640     depth_height:=576     depth_fps:=30     enable_color:=true     enable_depth:=true     color_format:=RGB888

Provokes the driver publishing the static tf with Nan values, thus giving in another terminal warnings and errors since this other module using the camera information uses tf2 as well.

Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "camera_accel_frame" from authority "default_authority" because of a nan value in the transform (-nan -0.000000 -inf) (-nan -nan -nan -nan)
         at line 235 in ./src/buffer_core.cpp
Error:   TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "camera_accel_frame" from authority "default_authority" because of an invalid quaternion in the transform (-nan -nan -nan -nan)
         at line 254 in ./src/buffer_core.cpp
Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "camera_gyro_frame" from authority "default_authority" because of a nan value in the transform (-nan -0.000000 -inf) (-nan -nan -nan -nan)

Expected output

When launching the driver, it is able to properly calculate the transformations without any problem related to tf2.

Actual output

When testing the latest 1.58 version from 1.57 the following issue arises:

[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [component_container-1]: process started with pid [169690]
[component_container-1] [INFO] [1719318129.549749307] [camera.camera_container]: Load Library: /ros/orbecc_ws/install/orbbec_camera/lib/liborbbec_camera.so
[component_container-1] [INFO] [1719318129.626721050] [camera.camera_container]: Found class: rclcpp_components::NodeFactoryTemplate<orbbec_camera::OBCameraNodeDriver>
[component_container-1] [INFO] [1719318129.627190855] [camera.camera_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<orbbec_camera::OBCameraNodeDriver>
[component_container-1] [06/25 14:22:09.634843][info][169690][Context.cpp:68] Context created with config: /ros/orbecc_ws/install/orbbec_camera/share/orbbec_camera/config/OrbbecSDKConfig_v1.0.xml
[component_container-1] [06/25 14:22:09.635234][info][169690][Context.cpp:73] Work directory=/ros/orbecc_ws, SDK version=v1.10.8-20240604-d1671a3
[component_container-1] [06/25 14:22:09.635417][info][169690][LinuxPal.cpp:32] createObPal: create LinuxPal!
[component_container-1] [06/25 14:22:09.733248][info][169690][LinuxPal.cpp:143] Create PollingDeviceWatcher!
[component_container-1] [06/25 14:22:09.733263][info][169690][DeviceManager.cpp:15] Current found device(s): (1)
[component_container-1] [06/25 14:22:09.733266][info][169690][DeviceManager.cpp:24]     - Name: Femto Bolt, PID: 0x066b, SN/ID , Connection: USB3.1
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/camera/camera' in container '/camera/camera_container'
[component_container-1] [INFO] [1719318130.233909518] [camera.camera]: Connecting to the default device
[component_container-1] [INFO] [1719318131.715367454] [camera.camera]: use_hardware_time: true
[component_container-1] [INFO] [1719318131.725714419] [camera.camera]: default_soft_filter_max_diff: 300
[component_container-1] [INFO] [1719318131.725797215] [camera.camera]: default_soft_filter_speckle_size: 25
[component_container-1] [INFO] [1719318131.726051642] [camera.camera]:  stream depth is enabled - width: 640, height: 576, fps: 30, Format: OB_FORMAT_Y16
[component_container-1] [INFO] [1719318131.726226795] [camera.camera]:  stream ir is enabled - width: 640, height: 576, fps: 30, Format: OB_FORMAT_Y16
[component_container-1] [INFO] [1719318131.726889758] [camera.camera]:  stream color is enabled - width: 1280, height: 960, fps: 30, Format: OB_FORMAT_RGB
[component_container-1] [INFO] [1719318131.726939351] [camera.camera]: stream gyro full scale range 1000dps sample rate 200hz
[component_container-1] [INFO] [1719318131.726969128] [camera.camera]: stream accel full scale range 4g sample rate 200hz
[component_container-1] [INFO] [1719318131.746917326] [camera.camera]: Publish diagnostics every 1 seconds
[component_container-1] [INFO] [1719318131.903412663] [camera.camera]: start accel stream with range: 4g,rate:200hz, and start gyro stream with range:1000dps,rate:200hz
[component_container-1] [INFO] [1719318131.903510686] [camera.camera]: set align mode to ALIGN_D2C_SW_MODE
[component_container-1] [INFO] [1719318131.903526783] [camera.camera]: enable depth scale ON
[component_container-1] [INFO] [1719318131.903539537] [camera.camera]: Enable depth stream
[component_container-1] [INFO] [1719318131.903552844] [camera.camera]: Stream depth width: 640 height: 576 fps: 30 format: Y16
[component_container-1] [INFO] [1719318131.903563177] [camera.camera]: Enable ir stream
[component_container-1] [INFO] [1719318131.903572913] [camera.camera]: Stream ir width: 640 height: 576 fps: 30 format: Y16
[component_container-1] [INFO] [1719318131.903581809] [camera.camera]: Enable color stream
[component_container-1] [INFO] [1719318131.903591030] [camera.camera]: Stream color width: 1280 height: 960 fps: 30 format: RGB888
[component_container-1] [INFO] [1719318133.217444099] [camera.camera]: Enable frame sync
[component_container-1] [INFO] [1719318133.217571298] [camera.camera]: Device Femto Bolt connected
[component_container-1] [INFO] [1719318133.217585721] [camera.camera]: Serial number: 
[component_container-1] [INFO] [1719318133.217597695] [camera.camera]: Firmware version: 1.0.9
[component_container-1] [INFO] [1719318133.217620809] [camera.camera]: Hardware version: 1.2
[component_container-1] [INFO] [1719318133.217629061] [camera.camera]: device unique id: 4-3-9
[component_container-1] [INFO] [1719318133.217641691] [camera.camera]: Current node pid: 169690
[component_container-1] [INFO] [1719318133.809700533] [camera.camera]: Publishing static transform from camera_link to ir
[component_container-1] [INFO] [1719318133.809749210] [camera.camera]: Translation 0, 0, 0
[component_container-1] [INFO] [1719318133.809757228] [camera.camera]: Rotation 0, 0, 0, 1
[component_container-1] [INFO] [1719318133.809770495] [camera.camera]: Publishing static transform from camera_link to color
[component_container-1] [INFO] [1719318133.809778266] [camera.camera]: Translation 32.6763, 1.09422, -2.14453
[component_container-1] [INFO] [1719318133.809785533] [camera.camera]: Rotation -0.00317307, 0.053004, -5.35962e-05, 0.997891
[component_container-1] [INFO] [1719318133.809791528] [camera.camera]: Publishing static transform from camera_link to depth
[component_container-1] [INFO] [1719318133.809797083] [camera.camera]: Translation 0, 0, 0
[component_container-1] [INFO] [1719318133.809802674] [camera.camera]: Rotation 0, 0, 0, 1
[component_container-1] [INFO] [1719318133.809810151] [camera.camera]: Publishing static transform from camera_link to accel
[component_container-1] [INFO] [1719318133.809815589] [camera.camera]: Translation 0, inf, -nan
[component_container-1] [INFO] [1719318133.809820975] [camera.camera]: Rotation -nan, -nan, -nan, -nan
[component_container-1] [INFO] [1719318133.809827225] [camera.camera]: Publishing static transform from camera_link to gyro
[component_container-1] [INFO] [1719318133.809832147] [camera.camera]: Translation 0, inf, -nan
[component_container-1] [INFO] [1719318133.809837055] [camera.camera]: Rotation -nan, -nan, -nan, -nan
[component_container-1] [WARN] [1719318133.809989002] [camera.camera]: Publishing dynamic camera transforms (/tf) at 10 Hz

All the topics work as expected and the pcd is being published

Thanks in advance

Danilrivero commented 4 months ago

Hello,

I have noticed that in some past commits there have been some fixes related to static transforms published such as https://github.com/orbbec/OrbbecSDK_ROS2/commit/ee5f7d7253f2af3d70cf7e43aa7aff72b3144c12. The driver still produces these errors. Is there any information related to this?

Thanks in advance.

Danilrivero commented 2 months ago

For traceability I have been working lately with the latest commits and it still does happen. The calculation made returns a NaN value which can be quite annoying when working with other nodes using the tf tree for transformations between frames.