ROS2 Realsense node stops publishing image data after calling up other nodes
Hi, I'm using ROS2 and realsense D435i to implement a navigation application. I modified this example which brings up the realsense node, nvblox, and VSLAM. It runs well when all nodes are in the same machine. I divided them, such as the realsense node only running on the jetson board on my robots, nvblox, and VSLAM runs in the edge server. The robot and edge server are connected with ethernet cables. Please note that I have tested that they can ping and use the talker listener examples from ROS2 well.
Problem: When I start the realsense node at the jetson board side, I can see topics and image data in the edge server. But when I bring up the nvblox and VSLAM nodes, the edge side doesn't see the image, but the topic name is still there, and the jetson side can see the image data.
Is there any mistake in my launch files, or is something missing in the network setup?
Below are my launch files.
The nvr_debug_jetson.launch.py file:
# license removed for brevity
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.descriptions import ComposableNode
from launch_ros.actions import ComposableNodeContainer
def generate_launch_description():
realsense_config_file_path = os.path.join(
get_package_share_directory("nvblox_examples_bringup"),
"config",
"realsense.yaml",
)
realsense_node = ComposableNode(
namespace="camera",
package="realsense2_camera",
plugin="realsense2_camera::RealSenseNodeFactory",
parameters=[realsense_config_file_path],
)
realsense_splitter_node = ComposableNode(
namespace="camera",
name="realsense_splitter_node",
package="realsense_splitter",
plugin="nvblox::RealsenseSplitterNode",
parameters=[{"input_qos": "SENSOR_DATA", "output_qos": "SENSOR_DATA"}],
remappings=[
("input/infra_1", "/camera/infra1/image_rect_raw"),
("input/infra_1_metadata", "/camera/infra1/metadata"),
("input/infra_2", "/camera/infra2/image_rect_raw"),
("input/infra_2_metadata", "/camera/infra2/metadata"),
("input/depth", "/camera/depth/image_rect_raw"),
("input/depth_metadata", "/camera/depth/metadata"),
("input/pointcloud", "/camera/depth/color/points"),
("input/pointcloud_metadata", "/camera/depth/metadata"),
],
)
realsense_container = ComposableNodeContainer(
name="realsense_container",
namespace="",
package="rclcpp_components",
executable="component_container",
composable_node_descriptions=[realsense_node, realsense_splitter_node],
output="screen",
)
base_link_tf_node = Node(
package="tf2_ros",
executable="static_transform_publisher",
arguments=["0.16", "0", "0.11", "0", "0", "0", "1", "base_link", "camera_link"],
)
return LaunchDescription([realsense_container, base_link_tf_node])
The nvr_debug_edge.launch.py file:
# license removed for brevity
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
visual_slam_node = Node(
name="visual_slam_node",
package="isaac_ros_visual_slam",
executable="isaac_ros_visual_slam",
parameters=[
{
"enable_rectified_pose": True,
"denoise_input_images": False,
"rectified_images": True,
"enable_debug_mode": False,
"debug_dump_path": "/tmp/vslam",
"enable_slam_visualization": True,
"enable_landmarks_view": True,
"enable_observations_view": True,
"map_frame": "map",
"odom_frame": "odom",
"base_frame": "base_link",
"input_left_camera_frame": "camera_infra1_frame",
"input_right_camera_frame": "camera_infra2_frame",
"enable_localization_n_mapping": True,
"publish_odom_to_base_tf": True,
"publish_map_to_odom_tf": True,
}
],
remappings=[
("stereo_camera/left/image", "/camera/infra1/image_rect_raw"),
("stereo_camera/left/camera_info", "/camera/infra1/camera_info"),
("stereo_camera/right/image", "/camera/infra2/image_rect_raw"),
("stereo_camera/right/camera_info", "/camera/infra2/camera_info"),
],
)
nvblox_config = DeclareLaunchArgument(
"nvblox_config",
default_value=os.path.join(
get_package_share_directory("nvblox_examples_bringup"),
"config",
"nvblox.yaml",
),
)
nvblox_node = Node(
package="nvblox_ros",
executable="nvblox_node",
parameters=[LaunchConfiguration("nvblox_config")],
output="screen",
remappings=[
("depth/camera_info", "/camera/depth/camera_info"),
("depth/image", "/camera/realsense_splitter_node/output/depth"),
("color/camera_info", "/camera/color/camera_info"),
("color/image", "/camera/color/image_raw"),
],
)
rviz_config_path = os.path.join(
get_package_share_directory("nvblox_examples_bringup"),
"config",
"nvblox_vslam_realsense.rviz",
)
print(rviz_config_path)
rviz = Node(
package="rviz2",
executable="rviz2",
arguments=["-d", rviz_config_path],
output="screen",
)
return LaunchDescription([nvblox_config, visual_slam_node, nvblox_node, rviz])
Below is the output of the realsense node. It appears that some errors are on, but I don't know if this problem has to do with it.
admin@ubuntu:/workspaces/isaac_ros-dev$ source install/setup.bash
admin@ubuntu:/workspaces/isaac_ros-dev$ ros2 launch nvblox_examples_bringup nvr_debug.launch.py
[INFO] [launch]: All log files can be found below /home/admin/.ros/log/2022-10-13-10-58-40-532074-ubuntu-30285
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [component_container-1]: process started with pid [30298]
[INFO] [static_transform_publisher-2]: process started with pid [30300]
[static_transform_publisher-2] [WARN] [1665658721.809580602] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-2] [INFO] [1665658721.908011870] [static_transform_publisher_okFmVYzYxSyDHaoM]: Spinning until stopped - publishing transform
[static_transform_publisher-2] translation: ('0.160000', '0.000000', '0.110000')
[static_transform_publisher-2] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-2] from 'base_link' to 'camera_link'
[component_container-1] [INFO] [1665658722.208643569] [realsense_container]: Load Library: /workspaces/isaac_ros-dev/install/realsense2_camera/lib/librealsense2_camera.so
[component_container-1] [INFO] [1665658722.362127880] [realsense_container]: Found class: rclcpp_components::NodeFactoryTemplate<realsense2_camera::RealSenseNodeFactory>
[component_container-1] [INFO] [1665658722.362456857] [realsense_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<realsense2_camera::RealSenseNodeFactory>
[component_container-1] [INFO] [1665658722.533116839] [camera.camera]: RealSense ROS v3.2.3
[component_container-1] [INFO] [1665658722.533400565] [camera.camera]: Built with LibRealSense v2.51.1
[component_container-1] [INFO] [1665658722.533531771] [camera.camera]: Running with LibRealSense v2.51.1
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/camera/camera' in container '/realsense_container'
[component_container-1] [INFO] [1665658722.551024226] [realsense_container]: Load Library: /workspaces/isaac_ros-dev/install/realsense_splitter/lib/librealsense_splitter_component.so
[component_container-1] [INFO] [1665658722.567078113] [realsense_container]: Found class: rclcpp_components::NodeFactoryTemplatenvblox::RealsenseSplitterNode
[component_container-1] [INFO] [1665658722.567578362] [realsense_container]: Instantiate class: rclcpp_components::NodeFactoryTemplatenvblox::RealsenseSplitterNode
[component_container-1] [INFO] [1665658722.606632370] [camera.realsense_splitter_node]: Creating a RealsenseSplitterNode().
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/camera/realsense_splitter_node' in container '/realsense_container'
[component_container-1] [INFO] [1665658723.096249348] [camera.camera]: Device with serial number 135122070687 was found.
[component_container-1]
[component_container-1] [INFO] [1665658723.096623606] [camera.camera]: Device with physical ID 2-4.1.3-5 was found.
[component_container-1] [INFO] [1665658723.096826272] [camera.camera]: Device with name Intel RealSense D435I was found.
[component_container-1] [INFO] [1665658723.098514932] [camera.camera]: Device with port number 2-4.1.3 was found.
[component_container-1] [INFO] [1665658723.098679581] [camera.camera]: Device USB type: 3.2
[component_container-1] [INFO] [1665658723.144959324] [camera.camera]: getParameters...
[component_container-1] [INFO] [1665658723.205539107] [camera.camera]: setupDevice...
[component_container-1] [INFO] [1665658723.205834930] [camera.camera]: JSON file is not provided
[component_container-1] [INFO] [1665658723.205979449] [camera.camera]: Device Name: Intel RealSense D435I
[component_container-1] [INFO] [1665658723.206071998] [camera.camera]: Device physical port: 2-4.1.3-5
[component_container-1] [INFO] [1665658723.206209605] [camera.camera]: Device FW version: 05.13.00.50
[component_container-1] [INFO] [1665658723.206289961] [camera.camera]: Device Product ID: 0x0B3A
[component_container-1] [INFO] [1665658723.206357612] [camera.camera]: Enable PointCloud: Off
[component_container-1] [INFO] [1665658723.206418223] [camera.camera]: Align Depth: Off
[component_container-1] [INFO] [1665658723.206476114] [camera.camera]: Sync Mode: Off
[component_container-1] [INFO] [1665658723.206601240] [camera.camera]: Device Sensors:
[component_container-1] [INFO] [1665658723.375074489] [camera.camera]: Stereo Module was found.
[component_container-1] [INFO] [1665658723.508486890] [camera.camera]: RGB Camera was found.
[component_container-1] [INFO] [1665658723.510156733] [camera.camera]: Motion Module was found.
[component_container-1] [INFO] [1665658723.510575378] [camera.camera]: (Infrared, 0) sensor isn't supported by current device! -- Skipping...
[component_container-1] [INFO] [1665658723.510700760] [camera.camera]: (Fisheye, 0) sensor isn't supported by current device! -- Skipping...
[component_container-1] [INFO] [1665658723.510776220] [camera.camera]: (Fisheye, 1) sensor isn't supported by current device! -- Skipping...
[component_container-1] [INFO] [1665658723.510844447] [camera.camera]: (Fisheye, 2) sensor isn't supported by current device! -- Skipping...
[component_container-1] [INFO] [1665658723.510929123] [camera.camera]: (Pose, 0) sensor isn't supported by current device! -- Skipping...
[component_container-1] [INFO] [1665658723.511013384] [camera.camera]: (Confidence, 0) sensor isn't supported by current device! -- Skipping...
[component_container-1] [INFO] [1665658723.511090251] [camera.camera]: num_filters: 0
[component_container-1] [INFO] [1665658723.511151374] [camera.camera]: Setting Dynamic reconfig parameters.
[component_container-1] [INFO] [1665658729.268175134] [camera.camera]: Done Setting Dynamic reconfig parameters.
[component_container-1] [INFO] [1665658729.278360921] [camera.camera]: depth stream is enabled - width: 640, height: 480, fps: 30, Format: Z16
[component_container-1] [INFO] [1665658729.280400767] [camera.camera]: infra1 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8
[component_container-1] [INFO] [1665658729.288298792] [camera.camera]: infra2 stream is enabled - width: 640, height: 480, fps: 30, Format: Y8
[component_container-1] [INFO] [1665658729.293281216] [camera.camera]: color stream is enabled - width: 640, height: 480, fps: 30, Format: RGB8
[component_container-1] [INFO] [1665658729.293696052] [camera.camera]: setupPublishers...
[component_container-1] [INFO] [1665658729.697329774] [camera.camera]: setupStreams...
[component_container-1] [WARN] [1665658730.271933798] [camera.camera]: Hardware Notification:Depth stream start failure,1.66566e+12,Error,Hardware Error
[component_container-1] [INFO] [1665658730.398371067] [camera.camera]: SELECTED BASE:Depth, 0
[component_container-1] [INFO] [1665658730.421108359] [camera.camera]: Device Serial No: 135122070687
[component_container-1] [INFO] [1665658730.421718149] [camera.camera]: RealSense Node Is Up!
[component_container-1] [WARN] [1665658730.527182918] [camera.camera]:
[component_container-1] 13/10 10:58:50,529 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:58:50,580 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:58:50,742 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:58:50,793 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:58:50,930 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:58:50,985 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:58:51,014 ERROR [281472225241392] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 130
[component_container-1] 13/10 10:58:51,243 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:59:17,944 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:59:23,003 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:59:30,066 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:59:41,357 ERROR [281472200063280] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 131
[component_container-1] 13/10 10:59:47,687 ERROR [281472200063280] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 131
[component_container-1] 13/10 10:59:56,148 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 10:59:57,015 ERROR [281472200063280] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 131
[component_container-1] 13/10 11:00:02,345 ERROR [281472200063280] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 131
[component_container-1] 13/10 11:00:17,683 ERROR [281472795650352] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 132
[component_container-1] 13/10 11:00:18,003 ERROR [281472200063280] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 131
[component_container-1] 13/10 11:00:24,238 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 11:00:26,291 WARNING [281473113725232] (messenger-libusb.cpp:42) control_transfer returned error, index: 768, error: Resource temporarily unavailable, number: 11
[component_container-1] 13/10 11:00:29,677 ERROR [281472795650352] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 132
[component_container-1] [WARN] [1665658831.352656482] [camera.camera]: Hardware Notification:Right MIPI error,1.66566e+12,Error,Hardware Error
[component_container-1] 13/10 11:00:53,983 ERROR [281472200063280] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 131
[component_container-1] 13/10 11:01:01,324 ERROR [281472795650352] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 132
[component_container-1] [WARN] [1665658862.374305032] [camera.camera]: Hardware Notification:Right MIPI error,1.66566e+12,Error,Hardware Error
[component_container-1] 13/10 11:01:20,647 ERROR [281472795650352] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 132
[component_container-1] 13/10 11:01:22,313 ERROR [281472795650352] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 132
[component_container-1] 13/10 11:01:24,299 ERROR [281472200063280] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 131
[component_container-1] 13/10 11:01:30,642 ERROR [281472795650352] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 132
[component_container-1] 13/10 11:01:35,639 ERROR [281472795650352] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 132
[component_container-1] 13/10 11:01:40,291 ERROR [281472200063280] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 131
[component_container-1] 13/10 11:01:40,637 ERROR [281472795650352] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 132
[component_container-1] [WARN] [1665658901.400340923] [camera.camera]: Hardware Notification:Right MIPI error,1.66566e+12,Error,Hardware Error
[component_container-1] 13/10 11:01:43,956 ERROR [281472200063280] (uvc-streamer.cpp:106) uvc streamer watchdog triggered on endpoint: 131
Asked by chivas1000 on 2022-10-13 05:55:46 UTC
Comments
Network seem to be stuck after I bring up the VSLAM and NVBLOX nodes on server, because after I launch realsense on jetson, the server receive topic and data
But after I launched vslam and nvblox, camera data doesn't avaliable on my server, also I run ros2 run demo_nodes_cpp talker at jetson and ros2 run demo_nodes_py listener on server, server does not receive too.
I assume that the launch of the server side blocked the network and they can't see each other then.
Asked by chivas1000 on 2022-10-13 06:29:14 UTC
noting that the nodes on edges and servers are all running in the container, so is that the NAT issues?
Asked by chivas1000 on 2022-10-13 07:30:40 UTC
Updates:
When I started only realsense node at jetson container, and ros2 topic echo depth topic on the server, it only shows the first frame and stops, neither other image topics, but I ros2 topic echo at the jetson, the depth can shows.
Another tips is that I installed my system in the sd card at jetson, together with the error logs occured by realsense node, so is that the sd card I/O bottlenecked the buffer of transfer which disobeyed the QoS and dropped the connection?
while my jetson can receive about 25fps messages, the server only receives 3 messages and stopped:
so do the inferred images, when I only echo inferred 1 at server, it receives and bandwidth is about 9MB/s 9.22 MB/s from 100 messages Message size mean: 0.31 MB min: 0.31 MB max: 0.31 MB
but when I echo another inferred 2, these two topics dropped and stopped transmitting, so I think when bw over 10MB/s, the jetson can't handle transmission and dropped connection.
Asked by chivas1000 on 2022-10-14 04:40:28 UTC
I think the problem might be similar to this:
https://support.intelrealsense.com/hc/en-us/community/posts/4939637103891--messenger-libusb-cpp-42-control-transfer-returned-error-index-768-error-Resource-temporarily-unavailable-number-11?sort_by=votes
and
https://answers.ros.org/question/389347/ros-dropping-messages-how-to-predict-it/
so is that the hardware issue and if it can fixed with switching the system from sd card to SSD and reducing depth frame rate? Or, I might just using an Intel nuc to run realsense node and transfer image topics since it better deal with realsense camera?
I'm waiting for the ssd and putting on it to see if it can fix.
==========================================================
here is my realsense yaml files:
device_type: '' serial_no: '' usb_port_id: ''
rgb_camera: profile: '640x480x15' color_qos: "SENSOR_DATA"
depth_module: profile: '640x480x15' emitter_enabled: 1 emitter_on_off: true depth_qos: "SENSOR_DATA" depth_info_qos: "SYSTEM_DEFAULT"
infr
Asked by chivas1000 on 2022-10-14 04:43:20 UTC
sorry for comments one, the talker and listener examples work, it might just because I opened it with 4 image topics transferring which dropped the connection and so do the talker and listener examples. Anyway, I supposed that network setup is good now and the issue are more likely to do with realsense node.
Asked by chivas1000 on 2022-10-14 05:08:25 UTC