search

IntelRealSense / realsense-ros

ROS Wrapper for Intel(R) RealSense(TM) Cameras
http://wiki.ros.org/RealSense
Apache License 2.0
2.58k stars 1.76k forks source link

The realsense ROS2 node sometimes launches but fails to publish data #1980

Closed pepisg closed 3 years ago

pepisg commented 3 years ago

Hi!

I'm using your ros2 package with an intel D435i. This is our setup: RealsenseROS: v3.2.1 LibRealSense: v2.45.0 Device FW: v05.12.12.100 Platform: Jetson Xavier AGX OS: Ubuntu 18 / Docker, L4T v32.4.2

It happens sometimes that the node launches and the corresponding process is in execution but none of the topics or services that the node should publish/offer are available. Our theory is that the execution of the node gets stuck somewhere in the publishTopics function and thus it is not able to set everything up.

This, however, does not happen always.

Thanks in advance for your help!

Serafadam commented 3 years ago

Hey, I'm having probably the same problems, and I've been trying to rebuild the code on different versions of librealsense, though no luck yet :disappointed:

I guess this is the same problem as in https://github.com/IntelRealSense/realsense-ros/issues/1962, https://github.com/IntelRealSense/realsense-ros/issues/1967 , https://github.com/IntelRealSense/realsense-ros/issues/1970

pepisg commented 3 years ago

Hi @Serafadam. Are you using also an ARM platform? Please let me know if you succeed with any other version. I will also let you know if we find out how to solve this problem.

Serafadam commented 3 years ago

Hi @pepisg, I'm using probably the same setup as you

RealSenseSupport commented 3 years ago

@pepisg Did you install librealsense by apt get install or building from source code? Is the streaming normal if you use RealSenseViewer for the same configuration such as streaming resolution and fps? Also could you please provide the log after ROS2 node launches till the issue happened? Thanks!

RealSenseSupport commented 3 years ago

@pepisg Do you still need support on this? Please note that the ticket will be closed if we don't hear from you for another 7 days.

Serafadam commented 3 years ago

@pepisg I've seen some better results after switching to Galactic & tuning DDS as in https://docs.ros.org/en/galactic/Guides/DDS-tuning.html, though a weird thing is that Pointcloud doesn't seem to be updated if there is a little difference in robot movement. For example, if I run the node against the wall and move the robot a little bit, nothing changes, even if the camera is looking at different part of the wall. But if I wave my hand in front of the camera, the frame updates

pepisg commented 3 years ago

@pepisg I've seen some better results after switching to Galactic & tuning DDS as in https://docs.ros.org/en/galactic/Guides/DDS-tuning.html, though a weird thing is that Pointcloud doesn't seem to be updated if there is a little difference in robot movement. For example, if I run the node against the wall and move the robot a little bit, nothing changes, even if the camera is looking at different part of the wall. But if I wave my hand in front of the camera, the frame updates

@Serafadam, sorry for the late reply. Unfortunately it is not possible for me to switch to Galactic. I have implemented a separate node to restart the realsense2_camera_node when this happens, however I will let you know if I find the root cause.

pepisg commented 3 years ago

@pepisg Do you still need support on this? Please note that the ticket will be closed if we don't hear from you for another 7 days.

Hi @RealSenseSupport, I'm building librealsense from source. I think I can not use RealSenseViewer as I can't access the desktop environment of the AGX, only the terminal. Please do not close the issue yet, I will try to gather some logs.

pepisg commented 3 years ago

Hi @RealSenseSupport, sorry for the late reply. I was finally able to retrieve some logs from the realsense ROS2 node when it launches but does not publish data on some topics: 

17.08.21 14:14:22 -> [INFO][init][241]: RealSense ROS v3.2.1
17.08.21 14:14:22 -> [INFO][init][242]: Built with LibRealSense v2.45.0
17.08.21 14:14:22 -> [INFO][init][243]: Running with LibRealSense v2.45.0
17.08.21 14:14:24 -> [INFO][getDevice][91]: Device with serial number 048122071310 was found.
17.08.21 14:14:24 -> 
17.08.21 14:14:24 -> [INFO][getDevice][94]: Device with physical ID 2-1-5 was found.
17.08.21 14:14:24 -> [INFO][getDevice][96]: Device with name Intel RealSense D435I was found.
17.08.21 14:14:24 -> [INFO][getDevice][114]: Device with port number 2-1 was found.
17.08.21 14:14:24 -> [INFO][getDevice][166]: Device USB type: 3.2
17.08.21 14:14:24 -> [INFO][getDevice][187]: Resetting device...
17.08.21 14:14:33 -> [INFO][getDevice][91]: Device with serial number 048122071310 was found.
17.08.21 14:14:33 -> 
17.08.21 14:14:33 -> [INFO][getDevice][94]: Device with physical ID 2-1-6 was found.
17.08.21 14:14:33 -> [INFO][getDevice][96]: Device with name Intel RealSense D435I was found.
17.08.21 14:14:33 -> [INFO][getDevice][114]: Device with port number 2-1 was found.
17.08.21 14:14:33 -> [INFO][getDevice][166]: Device USB type: 3.2
17.08.21 14:14:33 -> [INFO][getParameters][956]: getParameters...
17.08.21 14:14:33 -> [INFO][setupDevice][1066]: setupDevice...
17.08.21 14:14:33 -> [INFO][setupDevice][1090]: JSON file is not provided
17.08.21 14:14:33 -> [INFO][setupDevice][1093]: Device Name: Intel RealSense D435I
17.08.21 14:14:33 -> [INFO][setupDevice][1096]: Device Serial No: 048122071310
17.08.21 14:14:33 -> [INFO][setupDevice][1100]: Device physical port: 2-1-6
17.08.21 14:14:33 -> [INFO][setupDevice][1103]: Device FW version: 05.12.12.100
17.08.21 14:14:33 -> [INFO][setupDevice][1106]: Device Product ID: 0x0B3A
17.08.21 14:14:33 -> [INFO][setupDevice][1108]: Enable PointCloud: On
17.08.21 14:14:33 -> [INFO][setupDevice][1109]: Align Depth: On
17.08.21 14:14:33 -> [INFO][setupDevice][1110]: Sync Mode: On
17.08.21 14:14:33 -> [INFO][setupDevice][1140]: Device Sensors: 
17.08.21 14:14:34 -> [INFO][setupDevice][1178]: Stereo Module was found.
17.08.21 14:14:34 -> [INFO][setupDevice][1178]: RGB Camera was found.
17.08.21 14:14:34 -> [INFO][setupDevice][1178]: Motion Module was found.
17.08.21 14:14:34 -> [INFO][setupDevice][1187]: (Infrared, 0) sensor isn't supported by current device! -- Skipping...
17.08.21 14:14:34 -> [INFO][setupDevice][1187]: (Fisheye, 0) sensor isn't supported by current device! -- Skipping...
17.08.21 14:14:34 -> [INFO][setupDevice][1187]: (Pose, 0) sensor isn't supported by current device! -- Skipping...
17.08.21 14:14:34 -> [INFO][setupFilters][1523]: Add Filter: pointcloud
17.08.21 14:14:34 -> [INFO][setupFilters][1527]: num_filters: 2
17.08.21 14:14:34 -> [INFO][registerDynamicReconfigCb][795]: Setting Dynamic reconfig parameters.
17.08.21 14:14:36 -> [WARN][setParam][72]: Could not set param: stereo_module.auto_exposure_limit with 0Range: [1, 165000]: parameter 'stereo_module.auto_exposure_limit' could not be set: Parameter {} doesn't comply with integer range.
17.08.21 14:14:36 -> [WARN][setParam][72]: Could not set param: stereo_module.auto_gain_limit with 0Range: [16, 248]: parameter 'stereo_module.auto_gain_limit' could not be set: Parameter {} doesn't comply with integer range.
17.08.21 14:14:37 -> [INFO][registerDynamicReconfigCb][810]: Done Setting Dynamic reconfig parameters.
17.08.21 14:14:37 -> [INFO][enable_devices][1364]: depth stream is enabled - width: 640, height: 360, fps: 15, Format: Z16
17.08.21 14:14:37 -> [INFO][enable_devices][1364]: color stream is enabled - width: 640, height: 360, fps: 15, Format: RGB8
17.08.21 14:14:37 -> [INFO][enable_devices][1422]: gyro stream is enabled - fps: 200
17.08.21 14:14:37 -> [INFO][enable_devices][1422]: accel stream is enabled - fps: 63
17.08.21 14:14:37 -> [INFO][setupPublishers][1220]: setupPublishers...
17.08.21 14:14:37 -> [INFO][setupPublishers][1259]: Start publisher IMU
17.08.21 14:14:37 -> [INFO][setupStreams][2064]: setupStreams...
17.08.21 14:14:38 -> [WARN][setBaseTime][2038]: 
17.08.21 14:14:38 -> [WARN][setBaseTime][2041]: frame's time domain is HARDWARE_CLOCK. Timestamps may reset periodically.
17.08.21 14:14:39 -> [INFO][SetBaseStream][2290]: SELECTED BASE:Depth, 0
17.08.21 14:14:39 -> [INFO][publishTopics][358]: RealSense Node Is Up!
17.08.21 14:14:48 -> [INFO][publishPointCloud][2505]: publishing ordered pointcloud.

Please let me know if you see something that could point to the issue.

RealSenseSupport commented 3 years ago

@pepisg Could you please share the launch file you used? Below print is a little wired. Please check if FPS configuration for IR is different from depth. Thanks!

17.08.21 14:14:34 -> [INFO][setupDevice][1187]: (Infrared, 0) sensor isn't supported by current device! -- Skipping...

pepisg commented 3 years ago

Hi! These are the parameters that I'm using. We have added some custom parameters that however should not interfere with the image stream. 

configurable_parameters = [{'name': 'camera_name',                  'default': 'camera', 'description': 'camera unique name'},
                           {'name': 'serial_no',                    'default': "''", 'description': 'choose device by serial number'},
                           {'name': 'usb_port_id',                  'default': "''", 'description': 'choose device by usb port id'},
                           {'name': 'device_type',                  'default': "''", 'description': 'choose device by type'},
                           {'name': 'config_file',                  'default': "''", 'description': 'yaml config file'},
                           {'name': 'enable_pointcloud',            'default': 'true', 'description': 'enable pointcloud'},
                           {'name': 'unite_imu_method',             'default': 'copy', 'description': '[copy|linear_interpolation]'},                           
                           {'name': 'json_file_path',               'default': "''", 'description': 'allows advanced configuration'},                           
                           {'name': 'output',                       'default': 'screen', 'description': 'pipe node output [screen|log]'},                           
                           {'name': 'depth_width',                  'default': '640', 'description': 'depth image width'},                           
                           {'name': 'depth_height',                 'default': '480', 'description': 'depth image height'},                           
                           {'name': 'enable_depth',                 'default': 'true', 'description': 'enable depth stream'},
                           {'name': 'color_width',                  'default': '640', 'description': 'color image width'},                           
                           {'name': 'color_height',                 'default': '480', 'description': 'color image height'},                           
                           {'name': 'enable_color',                 'default': 'true', 'description': 'enable color stream'},
                           {'name': 'infra_width',                  'default': '640', 'description': 'infra width'},
                           {'name': 'infra_height',                 'default': '480', 'description': 'infra width'},
                           {'name': 'enable_infra1',                'default': 'false', 'description': 'enable infra1 stream'},
                           {'name': 'enable_infra2',                'default': 'false', 'description': 'enable infra2 stream'},
                           {'name': 'infra_rgb',                    'default': 'false', 'description': 'enable infra2 stream'},
                           {'name': 'fisheye_width',                'default': '-1', 'description': 'fisheye width'},
                           {'name': 'fisheye_height',               'default': '-1', 'description': 'fisheye width'},
                           {'name': 'enable_fisheye1',              'default': 'false', 'description': 'enable fisheye1 stream'},
                           {'name': 'enable_fisheye2',              'default': 'false', 'description': 'enable fisheye2 stream'},
                           {'name': 'confidence_width',             'default': '-1', 'description': 'depth image width'},                           
                           {'name': 'confidence_height',            'default': '-1', 'description': 'depth image height'},                           
                           {'name': 'enable_confidence',            'default': 'false', 'description': 'enable depth stream'},
                           {'name': 'fisheye_fps',                  'default': '-1.', 'description': ''},                           
                           {'name': 'depth_fps',                    'default': '30.', 'description': ''},                           
                           {'name': 'confidence_fps',               'default': '-1.', 'description': ''},                           
                           {'name': 'infra_fps',                    'default': '30.', 'description': ''},                           
                           {'name': 'color_fps',                    'default': '30.', 'description': ''},                           
                           {'name': 'gyro_fps',                     'default': '-1.', 'description': ''},                           
                           {'name': 'accel_fps',                    'default': '-1.', 'description': ''},    
                           {'name': 'color_qos',                    'default': 'SENSOR_DATA', 'description': 'QoS profile name'},    
                           {'name': 'confidence_qos',               'default': 'SENSOR_DATA', 'description': 'QoS profile name'},    
                           {'name': 'depth_qos',                    'default': 'SENSOR_DATA', 'description': 'QoS profile name'},    
                           {'name': 'fisheye_qos',                  'default': 'SENSOR_DATA', 'description': 'QoS profile name'},    
                           {'name': 'infra_qos',                    'default': 'SENSOR_DATA', 'description': 'QoS profile name'},    
                           {'name': 'enable_gyro',                  'default': 'false', 'description': ''},                           
                           {'name': 'enable_accel',                 'default': 'false', 'description': ''},                           
                           {'name': 'pointcloud_texture_stream',    'default': 'RS2_STREAM_COLOR', 'description': 'texture stream for pointcloud'},                           
                           {'name': 'pointcloud_texture_index',     'default': '0', 'description': 'texture stream index for pointcloud'},                          
                           {'name': 'enable_sync',                  'default': 'false', 'description': ''},                           
                           {'name': 'align_depth',                  'default': 'true', 'description': ''},                           
                           {'name': 'filters',                      'default': "''", 'description': ''},                           
                           {'name': 'clip_distance',                'default': '-2.', 'description': ''},                           
                           {'name': 'linear_accel_cov',             'default': '0.01', 'description': ''},                           
                           {'name': 'initial_reset',                'default': 'true', 'description': ''},                           
                           {'name': 'allow_no_texture_points',      'default': 'false', 'description': ''},                           
                           {'name': 'ordered_pc',                   'default': 'true', 'description': ''},                           
                           {'name': 'calib_odom_file',              'default': "''", 'description': "''"},                           
                           {'name': 'topic_odom_in',                'default': "''", 'description': 'topic for T265 wheel odometry'},
                           {'name': 'tf_publish_rate',              'default': '0.0', 'description': 'Rate of publishing static_tf'},
                           {'name': 'rosbag_filename',              'default': "''", 'description': 'A realsense bagfile to run from as a device'},
                           {'name': 'temporal.holes_fill',          'default': '0', 'description': 'Persistency mode'},
                           {'name': 'stereo_module.exposure.1',     'default': '7500', 'description': 'Initial value for hdr_merge filter'},
                           {'name': 'stereo_module.gain.1',         'default': '16', 'description': 'Initial value for hdr_merge filter'},
                           {'name': 'stereo_module.exposure.2',     'default': '1', 'description': 'Initial value for hdr_merge filter'},
                           {'name': 'stereo_module.gain.2',         'default': '16', 'description': 'Initial value for hdr_merge filter'},
                           {'name': 'color_virtual_cam',            'default': '-1', 'description': 'virtual camera to write color stream'},
                           {'name': 'texture_display_logs',         'default': '1', 'description': 'whether to show texture related logs'},                            
                           {'name': 'robot_base_frame',             'default': 'chassis', 'description': 'base frame for transform between camera and robot'},                           
                           {'name': 'camera_link_x',                'default': '0.21', 'description': 'x translation between base frame and camera'},                           
                           {'name': 'camera_link_y',                'default': '-0.041', 'description': 'y translation between base frame and camera'},                           
                           {'name': 'camera_link_z',                'default': '0.404', 'description': 'z translation between base frame and camera'},                           
                          ]

I think that this log you find weird is produced because in this line the image streams are enabled by default, and while you define streams called infra, fisheye and posehere they are not matched by any parameter provided by the launch file (which only provides control over infra1, infra2, fisheye1 and fisheye2), so they cannot be disabled (there should be some parameters called enable_infra, enable_fisheye and enable_pose). Later, this condition is met because the realsense D435i device object does not seem to yield any stream that matches those.

RealSenseSupport commented 3 years ago

@pepisg Thanks for the launch file and clarification! Which topic did you check when you saw it failed to publish data? Seen from the log, the camera was launched successfully. Besides, one observation that FPS was configured as 30fps but the log showed that FPS was 15. So could you please double check previous log was generated by this launch file? Thanks!

{'name': 'depth_fps', 'default': '30.', 'description': ''},

17.08.21 14:14:37 -> [INFO][enable_devices][1364]: depth stream is enabled - width: 640, height: 360, fps: 15, Format: Z16 17.08.21 14:14:37 -> [INFO][enable_devices][1364]: color stream is enabled - width: 640, height: 360, fps: 15, Format: RGB8

pepisg commented 3 years ago

I'm checking the topic /camera/color/image_raw. It's weird because as you say everything seems to launch normally but this topic does not publish while for instance the pointcloud one /camera/depth/color/points does. I will check the log you pointed out and let you know. Thanks!

EDIT: I checked and we are indeed setting the FPS to 15, sorry to not have provided this info before. That is the launch file that I'm using but I'm passing some parameters that are different from the default values:

All the other parameters have the default values.

RealSenseSupport commented 3 years ago

@pepisg Thanks for the update! How do you configure cmake options when you build librealsense from source? As you can't run RealSenseViewer, could you please try rs-color to see if you can get normal output?

pepisg commented 3 years ago

Hi! these are the flags I use with cmake. I will test what you suggest and add an update.

    cmake ../  
    -D CMAKE_CUDA_COMPILER="/usr/local/cuda/bin/nvcc" 
    -D FORCE_RSUSB_BACKEND=ON  
    -D BUILD_PYTHON_BINDINGS:bool=true 
    -D PYTHON_EXECUTABLE=/usr/bin/python3.8 
    -D CMAKE_BUILD_TYPE=release  
    -D BUILD_EXAMPLES=false  
    -D BUILD_GRAPHICAL_EXAMPLES=false 
    -D BUILD_WITH_CUDA:bool=true
RealSenseSupport commented 3 years ago

@pepisg Did you get chance to try rs-color and what's the result? Thanks!

RealSenseSupport commented 3 years ago

@pepisg Please build with -DBUILD_EXAMPLES=true and then you can run rs-color. Could you please have a try and update the result? Thanks!

pepisg commented 3 years ago

Hi! Sorry for the late reply. I already fixed the issue by creating a service to kill the realsense node and calling it from another node who constantly monitors the color stream frame rate. The root cause remains unknown, however I will close the issue as I have managed to fix the problem. Thanks for all your help!