Isaac ROS 3.2

Author: jaiveersinghNV

.gitignore

@@ -1,2 +1,3 @@
 # Ignore all pycache files
 **/__pycache__/**
+.vscode

README.md

@@ -101,9 +101,9 @@ outdoor scenes.
 
 ## Performance
 
-| Sample Graph<br/><br/>                                                                                                                                                                                                           | Input Size<br/><br/>      | Nova Carter<br/><br/>                                                                                                                                   |
-|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------|
-| [Multicam Visual SLAM Live Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_perceptor_nova_benchmark/scripts/isaac_ros_visual_slam_graph.py)<br/><br/><br/>4 Hawk Cameras<br/><br/> | 1200p<br/><br/><br/><br/> | [30.0 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_4_hawk_vslam_graph-carter_v2.4.json)<br/><br/><br/><br/> |
+| Sample Graph<br/><br/>                                                                                                                                                                                                           | Input Size<br/><br/>      | Nova Carter<br/><br/>                                                                                                                                       |
+|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|---------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| [Multicam Visual SLAM Live Graph](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/benchmarks/isaac_ros_perceptor_nova_benchmark/scripts/isaac_ros_visual_slam_graph.py)<br/><br/><br/>4 Hawk Cameras<br/><br/> | 1200p<br/><br/><br/><br/> | [30.1 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_4_hawk_vslam_graph-carter-v2.4-jp6.json)<br/><br/><br/><br/> |
 
 > [!Note]
 > This benchmark can only be run on a [Nova Orin](https://developer.nvidia.com/isaac/nova-orin) compatible system.
@@ -129,4 +129,4 @@ this repository.
 
 ## Latest
 
-Update 2024-09-26: Update for ZED compatibility
+Update 2024-12-10: Add support for multi-cam SLAM mode on stereo RGB cameras

isaac_ros_visual_slam/CMakeLists.txt

@@ -25,13 +25,15 @@ endif()
 execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE)
 message( STATUS "Architecture: ${ARCHITECTURE}" )
 
+find_package(Boost COMPONENTS thread REQUIRED)
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 # Dependencies
-find_package(Eigen3 REQUIRED)
 find_package(Threads REQUIRED)
+find_package(Eigen3 REQUIRED)
 include_directories(${EIGEN3_INCLUDE_DIR})
+include_directories(${isaac_common_INCLUDE_DIRS})
 
 # cuVSLAM (resolve path from ament_index)
 ament_index_get_resource(CUVSLAM_RELATIVE_PATH cuvslam isaac_ros_nitros)
@@ -44,6 +46,7 @@ foreach(PREFIX_PATH IN LISTS AMENT_INDEX_PREFIX_PATHS)
 endforeach()
 
 include_directories(${CUVSLAM}/include)
+
 add_library(cuvslam SHARED IMPORTED)
 set_property(TARGET cuvslam PROPERTY IMPORTED_LOCATION ${CUVSLAM}/lib/libcuvslam.so)
 
@@ -59,7 +62,7 @@ ament_auto_add_library(
   src/impl/visual_slam_impl.cpp
   src/impl/viz_helper.cpp
 )
-target_link_libraries(visual_slam_node cuvslam)
+target_link_libraries(visual_slam_node cuvslam Boost::thread)
 rclcpp_components_register_nodes(visual_slam_node "nvidia::isaac_ros::visual_slam::VisualSlamNode")
 set(node_plugins "${node_plugins}nvidia::isaac_ros::visual_slam::VisualSlamNode;$<TARGET_FILE:visual_slam_node>\n")
 
@@ -69,15 +72,19 @@ ament_auto_add_executable(${PROJECT_NAME}
 )
 target_link_libraries(${PROJECT_NAME} visual_slam_node)
 
+# API launcher executable
+install(PROGRAMS
+  ${CUVSLAM}/lib/cuvslam_api_launcher
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 # Install cuVSLAM
-install(DIRECTORY ${CUVSLAM}/lib DESTINATION share/cuvslam)
+install(FILES ${CUVSLAM}/lib/libcuvslam.so DESTINATION lib)
 
 set_target_properties(visual_slam_node PROPERTIES
   BUILD_WITH_INSTALL_RPATH TRUE
   BUILD_RPATH_USE_ORIGIN TRUE
-  INSTALL_RPATH_USE_LINK_PATH TRUE
-  INSTALL_RPATH "$ORIGIN/../share/cuvslam")
-
+  INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -89,22 +96,16 @@ if(BUILD_TESTING)
   endif()
 
   find_package(launch_testing_ament_cmake REQUIRED)
-  add_launch_test(test/isaac_ros_visual_slam_pol_single_cam_imu.py)
   add_launch_test(test/isaac_ros_visual_slam_pol_multi_cam_imu.py)
-  add_launch_test(test/isaac_ros_visual_slam_srv_reset.py)
+  add_launch_test(test/isaac_ros_visual_slam_pol_single_cam_imu.py)
   add_launch_test(test/isaac_ros_visual_slam_srv_get_all_poses.py)
+  add_launch_test(test/isaac_ros_visual_slam_srv_load_map.py)
+  # TODO(lgulich): Enable this test when we have better test data.
+  # add_launch_test(test/isaac_ros_visual_slam_srv_localize_in_map.py)
+  add_launch_test(test/isaac_ros_visual_slam_srv_reset.py)
+  add_launch_test(test/isaac_ros_visual_slam_srv_save_map.py)
   add_launch_test(test/isaac_ros_visual_slam_srv_set_slam_pose.py)
 
-  find_package(ament_cmake_gtest REQUIRED)
-  ament_add_gtest(${PROJECT_NAME}_test_message_buffer test/test_message_buffer.cpp)
-  target_include_directories(${PROJECT_NAME}_test_message_buffer PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>
-  )
-  ament_target_dependencies(${PROJECT_NAME}_test_message_buffer
-    std_msgs
-  )
-
   ament_add_gtest(${PROJECT_NAME}_test_message_stream_sequencer test/test_message_stream_sequencer.cpp)
   target_include_directories(${PROJECT_NAME}_test_message_stream_sequencer PUBLIC
     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -113,18 +114,12 @@ if(BUILD_TESTING)
   ament_target_dependencies(${PROJECT_NAME}_test_message_stream_sequencer
     std_msgs
   )
-
-  ament_add_gtest(${PROJECT_NAME}_test_message_stream_synchronizer
-    test/test_message_stream_synchronizer.cpp
-  )
-  target_include_directories(${PROJECT_NAME}_test_message_stream_synchronizer PUBLIC
-    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-    $<INSTALL_INTERFACE:include>
-  )
-  ament_target_dependencies(${PROJECT_NAME}_test_message_stream_synchronizer
-    rclcpp
-    geometry_msgs
-  )
 endif()
 
+
+# Embed versioning information into installed files
+ament_index_get_resource(ISAAC_ROS_COMMON_CMAKE_PATH isaac_ros_common_cmake_path isaac_ros_common)
+include("${ISAAC_ROS_COMMON_CMAKE_PATH}/isaac_ros_common-version-info.cmake")
+generate_version_info(${PROJECT_NAME})
+
 ament_auto_package(INSTALL_TO_SHARE config launch rviz)

isaac_ros_visual_slam/include/isaac_ros_visual_slam/impl/message_buffer.hpp

@@ -22,7 +22,8 @@
 #include <functional>
 #include <utility>
 #include <vector>
-#include "message_buffer.hpp"
+
+#include "isaac_common/messaging/message_buffer.hpp"
 
 namespace nvidia
 {
@@ -51,10 +52,9 @@ class MessageStreamSequencer
   void RegisterCallback(Callback callback);
 
 private:
-  // Buffer
-  MessageBuffer<T1> stream1_buff_;
+  isaac_common::messaging::MessageBuffer<T1> stream1_buff_;
   int64_t epsilon_stream1_;
-  MessageBuffer<T2> stream2_buff_;
+  isaac_common::messaging::MessageBuffer<T2> stream2_buff_;
   int64_t epsilon_stream2_;
   Callback registered_callback_ = [](const std::vector<T1> &, const T2 &) {};
 };