Merge pull request #49 from NVIDIA-ISAAC-ROS/release-dp2-hotfix-2

Hotfix to the nvblox realsense vslam example.

docs/tutorial-nvblox-vslam-realsense.md

@@ -4,6 +4,24 @@ This tutorial demonstrates how to perform depth-camera based reconstruction usin
 
 > Note: This tutorial has been tested with a Realsense D455/D435 camera connected to an x86 computer with an NVIDIA graphics card, as well as a Jetson Xavier AGX.
 
+## Realsense Camera Firmware
+
+This example is tested and compatible with realsense camera firmware version 5.13.0.50 which is available [here](https://dev.intelrealsense.com/docs/firmware-releases).
+
+> Note: We experienced issues with the latest realsense firmware (version 5.14 at the time of publishing). It's possible at some point that this starts working, but our recommendation is to install *exactly* 5.13.0.50.
+
+## Host System Setup
+
+We have found ROS2 message delivery to be unreliable under high load without some small modifications to the QoS profile (especially on weaker machines). Before running this example run
+```
+sudo sysctl -w net.core.rmem_max=8388608 net.core.rmem_default=8388608
+```
+However this only sets these parameters until reboot. To set them permanently run:
+```
+echo -e "net.core.rmem_max=8388608\nnet.core.rmem_default=8388608\n" | sudo tee /etc/sysctl.d/60-cyclonedds.conf
+```
+More details on DDS tuning can be found [here](https://docs.ros.org/en/rolling/How-To-Guides/DDS-tuning.html).
+
 ## Installing the Dependencies  
 
 1. Complete steps 1, 2, and 3 described in the [quickstart guide](../README.md#quickstart) to clone `isaac_ros_common` and

nvblox_examples/nvblox_examples_bringup/launch/nvblox_vslam_realsense.launch.py

@@ -75,10 +75,10 @@ def generate_launch_description():
     )
 
     # VSLAM
-    visual_slam_node = Node(
+    visual_slam_node = ComposableNode(
         name='visual_slam_node',
         package='isaac_ros_visual_slam',
-        executable='isaac_ros_visual_slam',
+        plugin='isaac_ros::visual_slam::VisualSlamNode',
         parameters=[{
                     'enable_rectified_pose': True,
                     'denoise_input_images': False,
@@ -91,8 +91,6 @@ def generate_launch_description():
                     '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,
@@ -104,6 +102,17 @@ def generate_launch_description():
                     ('stereo_camera/right/camera_info', '/camera/infra2/camera_info')]
     )
 
+    vslam_container = ComposableNodeContainer(
+        name='vslam_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            visual_slam_node
+        ],
+        output='screen'
+    )
+
     base_link_tf_node = Node(
         package='tf2_ros',
         executable='static_transform_publisher',
@@ -120,11 +129,11 @@ def generate_launch_description():
         )
     )
 
-    nvblox_node = Node(
+    nvblox_node = ComposableNode(
+        name='nvblox_node',
         package='nvblox_ros',
-        executable='nvblox_node',
+        plugin='nvblox::NvbloxNode',
         parameters=[LaunchConfiguration('nvblox_config')],
-        output='screen',
         remappings=[
             ("depth/camera_info", "/camera/depth/camera_info"),
             ("depth/image", "/camera/realsense_splitter_node/output/depth"),
@@ -133,23 +142,32 @@ def generate_launch_description():
         ]
     )
 
+    nvblox_container = ComposableNodeContainer(
+        name='nvblox_container',
+        namespace='',
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            nvblox_node
+        ],
+        output='screen'
+    )
+
     # RVIZ
     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([
-        realsense_container,
         nvblox_config,
-        visual_slam_node,
-        nvblox_node,
+        realsense_container,
+        vslam_container,
+        nvblox_container,
         base_link_tf_node,
         rviz
     ])

nvblox_ros/CMakeLists.txt

@@ -34,8 +34,8 @@ message( STATUS "CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}" )
 ################
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_auto REQUIRED)
-
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(sensor_msgs REQUIRED)
@@ -68,6 +68,8 @@ add_library(${PROJECT_NAME}_lib SHARED
 )
 target_link_libraries(${PROJECT_NAME}_lib nvblox::nvblox_lib nvblox::nvblox_eigen pthread)
 ament_target_dependencies(${PROJECT_NAME}_lib
+  rclcpp
+  rclcpp_components
   nvblox
   sensor_msgs
   geometry_msgs
@@ -86,6 +88,8 @@ target_include_directories(${PROJECT_NAME}_lib PUBLIC
 target_include_directories(${PROJECT_NAME}_lib BEFORE PRIVATE
   $<TARGET_PROPERTY:nvblox::nvblox_eigen,INTERFACE_INCLUDE_DIRECTORIES>)
 
+rclcpp_components_register_nodes(${PROJECT_NAME}_lib "nvblox::NvbloxNode")
+
 if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.18)
   get_target_property(CUDA_ARCHS nvblox::nvblox_lib CUDA_ARCHITECTURES)
   set_property(TARGET ${PROJECT_NAME}_lib APPEND PROPERTY CUDA_ARCHITECTURES ${CUDA_ARCHS})

nvblox_ros/include/nvblox_ros/nvblox_node.hpp

@@ -51,7 +51,7 @@ namespace nvblox
 class NvbloxNode : public rclcpp::Node
 {
 public:
-  NvbloxNode();
+  NvbloxNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
   virtual ~NvbloxNode() = default;
 
   // Callback functions. These just stick images in a queue.

nvblox_ros/package.xml

@@ -38,6 +38,7 @@
   <depend>libstatistics_collector</depend>
   <depend>nvblox_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>visualization_msgs</depend>

nvblox_ros/src/lib/nvblox_node.cpp

@@ -36,8 +36,8 @@
 namespace nvblox
 {
 
-NvbloxNode::NvbloxNode()
-: Node("nvblox_node"), transformer_(this)
+NvbloxNode::NvbloxNode(const rclcpp::NodeOptions & options)
+: Node("nvblox_node", options), transformer_(this)
 {
   // Declare & initialize the parameters.
   voxel_size_ = declare_parameter<float>("voxel_size", voxel_size_);
@@ -1002,3 +1002,7 @@ void NvbloxNode::loadMap(
 }
 
 }  // namespace nvblox
+
+// Register the node as a component
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(nvblox::NvbloxNode)