luxonis / depthai-ros

Official ROS Driver for DepthAI Sensors.
MIT License
254 stars 189 forks source link

[BUG] dynamic reconfigure options from ROS noetic not working from depthai_ros_driver #216

Open ladrians opened 1 year ago

ladrians commented 1 year ago

I want to try the dynamic reconfigure options from ROS noetic 1.15.15. I compiled from source the associated branch on a rpi4 (4gb RAM version). When running any of the depthai_ros_driver launch files nothing happens, I mean I do not see any image topic, just launching

roslaunch depthai_ros_driver camera.launch

Then I tried to change properties using rosrun rqt_reconfigure rqt_reconfigure. I see parameters buth cannot be applied o the device. For reference this is the output

[ INFO] [1675193138.661694059]: Loading nodelet /oak of type depthai_ros_driver/Camera to manager oak_nodelet_manager with the following remappings:
[ INFO] [1675193138.677505769]: waitForService: Service [/oak_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1675193138.813523905]: Initializing nodelet with 4 worker threads.
[ INFO] [1675193138.828776513]: waitForService: Service [/oak_nodelet_manager/load_nodelet] is now available.
[ INFO] [1675193139.656507067]: No ip/mxid specified, connecting to the next available device.
[ INFO] [1675193142.219597925]: Camera 19443010510B781300 connected!
[ INFO] [1675193142.221183069]: USB SPEED: SUPER
[ INFO] [1675193142.254171341]: Device type: OAK-D-PRO-W
[ INFO] [1675193142.262010780]: Pipeline type: RGBD
[ INFO] [1675193142.389339381]: NN Family: mobilenet
[ INFO] [1675193142.756870527]: Finished setting up pipeline.
[ INFO] [1675193145.300685273]: Camera ready!
[ WARN] [1675193175.794427226]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675193189.422791766]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675193198.352005455]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675193207.487119629]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':

Reboot and got worst

[ INFO] [1675193360.988806494]: Loading nodelet /oak of type depthai_ros_driver/Camera to manager oak_nodelet_manager with the following remappings:
[ INFO] [1675193361.003817450]: waitForService: Service [/oak_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1675193361.160894885]: Initializing nodelet with 4 worker threads.
[ INFO] [1675193361.177864164]: waitForService: Service [/oak_nodelet_manager/load_nodelet] is now available.
[ INFO] [1675193363.388163503]: No ip/mxid specified, connecting to the next available device.
[ INFO] [1675193366.012317079]: Camera 19443010510B781300 connected!
[ INFO] [1675193366.013953149]: USB SPEED: SUPER
[ INFO] [1675193366.045076944]: Device type: OAK-D-PRO-W
[ INFO] [1675193366.054258259]: Pipeline type: RGBD
[ INFO] [1675193366.195282230]: NN Family: mobilenet
[ INFO] [1675193366.735268933]: Finished setting up pipeline.
[FATAL] [1675193371.301629633]: Failed to load nodelet '/oak` of type `depthai_ros_driver/Camera` to manager `oak_nodelet_manager'
[oak_nodelet_manager-3] process has died [pid 2620, exit code -11, cmd /opt/ros/noetic/lib/nodelet/nodelet manager /nodelet_manager/load_nodelet:=oak/nodelet_manager/load_nodelet /nodelet_manager/unload_nodelet:=oak/nodelet_manager/unload_nodelet /nodelet_manager/list:=oak/nodelet_manager/list __name:=oak_nodelet_manager __log:=/home/ubuntu/.ros/log/8cfc9e54-a19d-11ed-bd57-e9b3939f6a43/oak_nodelet_manager-3.log].
log file: /home/ubuntu/.ros/log/8cfc9e54-a19d-11ed-bd57-e9b3939f6a43/oak_nodelet_manager-3*.log
================================================================================REQUIRED process [oak-4] has died!
process has died [pid 2621, exit code 255, cmd /opt/ros/noetic/lib/nodelet/nodelet load depthai_ros_driver/Camera oak_nodelet_manager __name:=oak __log:=/home/ubuntu/.ros/log/8cfc9e54-a19d-11ed-bd57-e9b3939f6a43/oak-4.log].
log file: /home/ubuntu/.ros/log/8cfc9e54-a19d-11ed-bd57-e9b3939f6a43/oak-4*.log
Initiating shutdown!
================================================================================

Any advice? attaching the log_system_information file, using Ubuntu 20.04.1 LTS. I want to tweak the parameters to use fewer resources, timeframes, rates and so on.

When using the depthai_examples launch samples I can connect to the device and receive information.

By the way, is it possible to run RGBD + pointcloud + some NN model at the same time?

regards,

Serafadam commented 1 year ago

Hi, which parameters did you try to set? Until packages are available on apt you can also try docker images docker run -it -v /dev/:/dev/ --privileged --network host -e DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix luxonis/depthai-ros:noetic-latest

ladrians commented 1 year ago

All the parameters have the same behavior, for example these ones using roslaunch depthai_ros_driver pointcloud.launch

camera_i_enable_ir
camera_i_floodlight_brightness
camera_i_floodlight_brightness
left_i_low_bandwidth

and the associated output

[ INFO] [1675936962.120925770]: Loading nodelet /oak of type depthai_ros_driver/Camera to manager nodelet_manager with the following remappings:
[ INFO] [1675936962.128763750]: waitForService: Service [/nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1675936962.242844709]: Initializing nodelet with 4 worker threads.
[ INFO] [1675936962.260282578]: waitForService: Service [/nodelet_manager/load_nodelet] is now available.
[ INFO] [1675936963.257968289]: No ip/mxid specified, connecting to the next available device.
[ INFO] [1675936965.834260629]: Camera 19443010510B781300 connected!
[ INFO] [1675936965.836232018]: USB SPEED: SUPER
[ INFO] [1675936965.877756787]: Device type: OAK-D-PRO-W
[ INFO] [1675936965.893070863]: Pipeline type: RGBD
[ INFO] [1675936966.040420695]: Finished setting up pipeline.
[19443010510B781300] [1.1.1] [1.703] [StereoDepth(3)] [warning] DepthAlign to camera LEFT/RIGHT not yet implemented, TODO
[ INFO] [1675936968.059794840]: Camera ready!
[ WARN] [1675937096.192720292]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675937183.789583768]: Reconfigure callback failed with exception Device already closed or disconnected:
[ WARN] [1675937184.860345845]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675937188.516019990]: Reconfigure callback failed with exception Device already closed or disconnected:
[ WARN] [1675937189.200205001]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675937207.948223497]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675937226.383379158]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675937227.436068062]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675937228.132881612]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675937257.108197253]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675937258.000093738]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675937258.561491411]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
[ WARN] [1675937259.171923299]: Reconfigure callback failed with exception Communication exception - possible device error/misconfiguration. Original message 'Couldn't write data to stream: 'rgb_control' (X_LINK_ERROR)':
...

a minor error on this file is to use /oak:.

My current status is that the nodes executes but using rqt_image_viewor rostopic hz <topicname> I cannot validate any message and no camera outout.

If using some sample like the following it works

roslaunch depthai_examples rgb_stereo_node.launch
ladrians commented 1 year ago

Found this warning in some execution

[ INFO] [1675938336.230254660]: Loading nodelet /oak of type depthai_ros_driver/Camera to manager nodelet_manager with the following remappings:
[ INFO] [1675938336.237741513]: waitForService: Service [/nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1675938336.347314914]: Initializing nodelet with 4 worker threads.
[ INFO] [1675938336.369463271]: waitForService: Service [/nodelet_manager/load_nodelet] is now available.
[ INFO] [1675938337.426001778]: No ip/mxid specified, connecting to the next available device.
[ INFO] [1675938340.029507572]: Camera 19443010510B781300 connected!
[ INFO] [1675938340.031340929]: USB SPEED: SUPER
[ INFO] [1675938340.071046342]: Device type: OAK-D-PRO-W
[ INFO] [1675938340.080966288]: Pipeline type: RGBD
[ INFO] [1675938340.226542426]: Finished setting up pipeline.
[19443010510B781300] [1.1.1] [1.694] [StereoDepth(3)] [warning] DepthAlign to camera LEFT/RIGHT not yet implemented, TODO
[2023-02-09 10:25:46.013] [warning] Monitor thread (device: 19443010510B781300 [1.1.1]) - ping was missed, closing the device connection
[ INFO] [1675938346.090792866]: Camera ready!

The connection to the device seems to be OK by dmesg

[ 1935.535078] usb 1-1.1: New USB device found, idVendor=03e7, idProduct=2485, bcdDevice= 0.01
[ 1935.535096] usb 1-1.1: New USB device strings: Mfr=1, Product=2, SerialNumber=3
[ 1935.535108] usb 1-1.1: Product: Movidius MyriadX
[ 1935.535120] usb 1-1.1: Manufacturer: Movidius Ltd.
[ 1935.535130] usb 1-1.1: SerialNumber: 03e72485
Serafadam commented 1 year ago

Hi :slightly_smiling_face:

By the way, is it possible to run RGBD + pointcloud + some NN model at the same time?

Sorry, I missed that. You could give custom camera config to rgbd_pcl.launch to enable additional NNs.

camera_i_enable_ir
camera_i_floodlight_brightness
camera_i_floodlight_brightness
left_i_low_bandwidth

parameters with names that start with i_ can't be modified using rqt_reconfigure unless camera is restarted (can be done using services stop_camera and start_camera

a minor error on [this file](https://github.com/luxonis/depthai-ros/blob/noetic/depthai_ros_driver/config/camera.yaml) is to use /oak:.

good catch, I'll fix that in next PR.

My current status is that the nodes executes but using rqt_image_view or rostopic hz <topicname> I cannot validate any message and no camera outout.

If using some sample like the following it works

Are you running everything on RPi? Or just the camera driver? Could you also share your depthai-core version? sudo apt install ros-noetic-depthai should print that out

ladrians commented 1 year ago

Hi there, I updated to the latest binaries

ros-noetic-depthai is already the newest version (2.20.2-1focal.20230202.200823).`

and from /opt/ros/noetic/share/depthai_ros_driver/package.xml is using

<package format="3">
  <name>depthai_ros_driver</name>
  <version>2.6.1</version>

But anyway trying to use it I get

[ INFO] [1678624448.257728832]: Loading nodelet /oak of type depthai_ros_driver/Camera to manager nodelet_manager with the following remappings:
[ INFO] [1678624448.274283797]: waitForService: Service [/nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1678624448.326164883]: Initializing nodelet with 4 worker threads.
[ INFO] [1678624448.346023867]: waitForService: Service [/nodelet_manager/load_nodelet] is now available.
[ INFO] [1678624449.361953190]: No ip/mxid specified, connecting to the next available device.
[ INFO] [1678624451.946302702]: Camera 19443010510B781300 connected!
[ INFO] [1678624451.947974526]: USB SPEED: SUPER
[ INFO] [1678624451.972648761]: Device type: OAK-D-PRO-W
[ INFO] [1678624451.983041852]: Pipeline type: RGBD
[FATAL] [1678624452.126232029]: Failed to load nodelet '/oak` of type `depthai_ros_driver/Camera` to manager `nodelet_manager'
================================================================================REQUIRED process [oak-6] has died!

So I switched to a compiled version from my workspace (which takes ages to compile on my rpi4). From there the package version is

<name>depthai_ros_driver</name>
<version>2.6.4</version>

I get a new error using the same camera.launch file

NODES
  /
    oak (nodelet/nodelet)
    oak_nodelet_manager (nodelet/nodelet)
    oak_state_publisher (robot_state_publisher/robot_state_publisher)
ROS_MASTER_URI=http://localhost:11311
process[oak_state_publisher-1]: started with pid [10632]
process[oak_nodelet_manager-2]: started with pid [10633]
process[oak-3]: started with pid [10634]
[ INFO] [1678625875.671615005]: Loading nodelet /oak of type depthai_ros_driver/Camera to manager oak_nodelet_manager with the following remappings:
[ INFO] [1678625875.685322791]: waitForService: Service [/oak_nodelet_manager/load_nodelet] could not connect to host [foxy01:57889], waiting...
[ INFO] [1678625875.801174749]: Initializing nodelet with 4 worker threads.
[ INFO] [1678625875.817611374]: waitForService: Service [/oak_nodelet_manager/load_nodelet] is now available.
[ INFO] [1678625876.470699419]: No ip/mxid/usb_id specified, connecting to the next available device.
[ INFO] [1678625879.319627679]: Camera with MXID: 19443010510B781300 and Name: 1.1.1 connected!
[ INFO] [1678625879.321955593]: USB SPEED: SUPER
[ INFO] [1678625879.350415381]: Device type: OAK-D-PRO-W
[ INFO] [1678625879.359850854]: Pipeline type: RGBD
double free or corruption (out)

No idea what to look for. I want to make it work the driver version to tweak parameter configuration. From the examples I got it working but is not what I need, ant hint on what to do with this new error?

Serafadam commented 1 year ago

Hi, is that error consistent, or happening at random? Could you also run export DEPTHAI_DEBUG=1 before launching node and paste the logs? Small comment regarding build time, we've made some refactoring that seems to cut it considerably, currently it's humble-based but as soon as it's merged it's going to be ported to Noetic, we apologize for current inconvenience. You can also download prebuilt Docker images from our Dockerhub.

ladrians commented 1 year ago

Yes, so far I could never test the driver option, I am adding the log oak_debug_enabled_20230315.txt thanks,

ladrians commented 1 year ago

We tested the same case on another machine (jetson orin) with similar results, adding the log too. oak_debug_enabled_orin_20230322.txt

ladrians commented 1 year ago

I changed to use docker with a Orin device, got the following error

[ INFO] [1679480544.451183545]: Loading nodelet /oak of type depthai_ros_driver/Camera to manager oak_nodelet_manager with the following remappings:
[ INFO] [1679480544.456610333]: waitForService: Service [/oak_nodelet_manager/load_nodelet] has not been advertised, waiting...
[ INFO] [1679480544.459577076]: Initializing nodelet with 12 worker threads.
[ INFO] [1679480544.479034266]: waitForService: Service [/oak_nodelet_manager/load_nodelet] is now available.
[ INFO] [1679480544.679992690]: No ip/mxid/usb_id specified, connecting to the next available device.
[ INFO] [1679480546.921318355]: Camera with MXID: 19443010510B781300 and Name: 1.4.2 connected!
[ INFO] [1679480546.922576746]: USB SPEED: SUPER
[ INFO] [1679480546.947531508]: Device type: OAK-D-PRO-W
[ INFO] [1679480546.952864630]: Pipeline type: RGBD
[ INFO] [1679480547.011256774]: NN Family: mobilenet
[ INFO] [1679480547.129832740]: Finished setting up pipeline.
[ INFO] [1679480548.717253588]: Camera ready!
[19443010510B781300] [1.4.2] [35.733] [system] [critical] Fatal error. Please report to developers. Log: 'Fatal error on MSS CPU: trap: 09, address: 801F5EC4' '0'

If I use export DEPTHAI_DEBUG=1 the camera loads topics, but cannot change anything from the dynamic_reconfigureutility, it just does not work, adding some logging on the attachment if it helps.

oak_debug_enabled_orin_docker_20230322.txt

Serafadam commented 1 year ago

Hi, thanks for the logs, I'm investigating further here. Just to be sure, demo script works fine for you? That last log entry got me wondering whether this could be something related to firmware.

ladrians commented 1 year ago

Yes it is working. Just in case I double checked now using an updated Jetson Xavier NX with JetPack 5.1.1, this is the output

$ python3 depthai_demo.py
Using depthai module from:  /home/jetson/.local/lib/python3.8/site-packages/depthai.cpython-38-aarch64-linux-gnu.so
Depthai version installed:  2.20.2.0
Setting up demo...
Available devices:
[0] 19443010510B781300 [X_LINK_UNBOOTED]
USB Connection speed: UsbSpeed.SUPER
IR drivers found on OAK-D Pro: ['LM3644 on bus 2']
 --> Go to the `Depth` tab to enable!
Downloading /home/jetson/.cache/blobconverter/mobilenet-ssd_openvino_2021.4_6shave.blob...
[==================================================]
Done
^CStopping demo...
=== TOTAL FPS ===
[color]: 18.2
[disparity]: 18.4
[disparityColor]: 18.4
[nn]: 17.9

Any idea? not sure how to update the firmware? Checked from here but could not find the directions, if you detail me I can try. Did the logs detailed any issue?

ladrians commented 1 year ago

I sync to 2.7.1 noetic version and the depthai_ros_driver start working!, but as I am working with a Jetson I needed to comment something to compile, could not find a solution to install opencv_contrib dependencies.

diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt
index 14ac771..d205153 100644
--- a/depthai_filters/CMakeLists.txt
+++ b/depthai_filters/CMakeLists.txt
@@ -54,7 +54,7 @@ ${OpenCV_INCLUDE_DIRS}
 add_library(${PROJECT_NAME} SHARED
     src/detection2d_overlay.cpp
     src/segmentation_overlay.cpp
-    src/wls_filter.cpp
+    #src/wls_filter.cpp
     src/utils.cpp
 )

Will it be a new dependency?

Serafadam commented 1 year ago

Hi, glad it works! Regarding the dependencies, which system version is that?

ladrians commented 1 year ago

Hi I have access again to the device, using JetPack 5.1.1, with

Description:    Ubuntu 20.04.6 LTS
Release:        20.04