Merge pull request #5 from christian-rauch/ros2

ROS 2 support
christian-rauch · Jan 3, 2024 · 97431f4 · 97431f4
2 parents 0a2f7e0 + 09590ec
commit 97431f4
Show file tree

Hide file tree

Showing 13 changed files with 243 additions and 61 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -4,7 +4,12 @@ project(multimotionfusion)
 set(CMAKE_CXX_STANDARD 17)
 
 # Warnings / Errors
-add_compile_options(-Wall -Werror -Wno-write-strings -Wno-deprecated-declarations)
+add_compile_options(
+    $<$<COMPILE_LANGUAGE:CXX>:-Wall>
+    $<$<COMPILE_LANGUAGE:CXX>:-Werror>
+    $<$<COMPILE_LANGUAGE:CXX>:-Wno-write-strings>
+    $<$<COMPILE_LANGUAGE:CXX>:-Wno-deprecated-declarations>
+)
 
 # Don't follow symlinks when FILE GLOB_RECURSE (and don't warn)
 cmake_policy(SET CMP0009 NEW)

diff --git a/Core/CMakeLists.txt b/Core/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.0.2)
 
-project(multimotionfusion)
+project(multimotionfusion LANGUAGES CXX CUDA)
 
 message(STATUS "Evaluating Core/CMAKE")
 
@@ -9,7 +9,6 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}")
 include(GNUInstallDirs)
 
 ## External packages
-find_package(CUDA REQUIRED)
 find_package(SuiteSparse REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(OpenMP)
@@ -65,7 +64,7 @@ message(STATUS "MULTIMOTIONFUSION_SHADER_DIR: ${MULTIMOTIONFUSION_SHADER_DIR}")
 
 ## Includes
 include_directories(${Pangolin_INCLUDE_DIRS})
-include_directories(${CUDA_INCLUDE_DIRS})
+include_directories(${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES})
 include_directories(${SUITESPARSE_INCLUDE_DIRS})
 include_directories(${PCL_INCLUDE_DIRS})
 include_directories(${OpenCV_INCLUDE_DIRS})
@@ -98,8 +97,6 @@ endif(CMAKE_BUILD_TYPE MATCHES Debug)
 # disable to use standard 'glTransformFeedbackVaryings' in palce of 'glTransformFeedbackVaryingsNV'
 add_definitions(-DNVIDIA_VARYINGS)
 
-CUDA_COMPILE(cuda_objs ${cuda})
-
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -msse -msse2 -msse3 -DSHADER_DIR=${MULTIMOTIONFUSION_SHADER_DIR}")
 
 if(CMAKE_BUILD_TYPE MATCHES Debug)
@@ -117,7 +114,6 @@ add_library(${PROJECT_NAME} SHARED
     ${utils_srcs}
     ${shader_srcs}
     ${cuda}
-    ${cuda_objs}
     ${containers}
     ${segmentation}
 )
@@ -126,7 +122,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
     ${CMAKE_CURRENT_SOURCE_DIR}
     ${ZLIB_INCLUDE_DIR}
     ${Pangolin_INCLUDE_DIRS}
-    ${CUDA_INCLUDE_DIRS}
+    ${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES}
     ${OPENNI2_INCLUDE_DIR}
     ${OpenCV_INCLUDE_DIRS}
     ${BOOST_INCLUDE_DIRS}
@@ -142,6 +138,9 @@ target_link_libraries(${PROJECT_NAME}
     super_point_inference
 )
 
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_11)
+set_target_properties(${PROJECT_NAME} PROPERTIES CUDA_SEPARABLE_COMPILATION ON)
+
 add_library(ransac STATIC "Utils/RigidRANSAC.cpp")
 target_link_libraries(ransac Eigen3::Eigen)
 

diff --git a/GUI/CMakeLists.txt b/GUI/CMakeLists.txt
@@ -1,22 +1,28 @@
 cmake_minimum_required(VERSION 3.5)
 
-project(multimotionfusion-gui)
+project(multimotionfusion-gui LANGUAGES CXX CUDA)
 
 set(CMAKE_CXX_STANDARD 17)
 
 message(STATUS "Evaluating GUI/CMAKE")
 
+if($ENV{ROS_VERSION})
+set(HASROS ON)
 if($ENV{ROS_VERSION} EQUAL 1)
     set(HASROS1 ON)
+    add_compile_options(-DROS1)
+elseif($ENV{ROS_VERSION} EQUAL 2)
+    set(HASROS2 ON)
+    add_compile_options(-DROS2)
+endif()
 endif()
 
 option(ROSBAG "rosbag reader" ${HASROS1})
 
-option(ROSNODE "read images live as ROS node" ${HASROS1})
+option(ROSNODE "read images live as ROS node" ${HASROS})
 
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}")
 find_package(ZLIB REQUIRED)
-find_package(CUDA REQUIRED)
 find_package(OpenNI2 REQUIRED)
 find_package(OpenCV REQUIRED )
 find_package(Boost REQUIRED filesystem)
@@ -32,10 +38,14 @@ if(ROSBAG)
 endif()
 
 if(ROSNODE)
-    find_package(roscpp REQUIRED)
+    if(HASROS1)
+        find_package(roscpp REQUIRED)
+        find_package(eigen_conversions REQUIRED)
+        add_compile_options(-DROSUI -DROSSTATE)
+    endif()
+    add_compile_options(-DROSREADER)
     find_package(image_transport REQUIRED)
     find_package(tf2_ros REQUIRED)
-    find_package(eigen_conversions REQUIRED)
     find_package(cob_srvs REQUIRED)
 endif()
 
@@ -46,7 +56,7 @@ if(ROSBAG OR ROSNODE)
 endif()
 
 include_directories(${ZLIB_INCLUDE_DIR})
-include_directories(${CUDA_INCLUDE_DIRS})
+include_directories(${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES})
 include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../Core)
 include_directories(${OPENNI2_INCLUDE_DIR})
 include_directories(${Pangolin_INCLUDE_DIRS})
@@ -55,7 +65,7 @@ include_directories(${OpenCV_INCLUDE_DIRS})
 
 message(STATUS "Include-directories: ")
 message(STATUS "ZLIB_INCLUDE_DIR: ${ZLIB_INCLUDE_DIR}")
-message(STATUS "CUDA_INCLUDE_DIRS: ${CUDA_INCLUDE_DIRS}")
+message(STATUS "CUDA_INCLUDE_DIRS: ${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES}")
 message(STATUS "OPENNI2_INCLUDE_DIR: ${OPENNI2_INCLUDE_DIR}")
 message(STATUS "CMAKE_CURRENT_SOURCE_DIR/../Core: ${${CMAKE_CURRENT_SOURCE_DIR}/../Core}")
 
@@ -101,7 +111,7 @@ target_include_directories(MultiMotionFusionTools PUBLIC
                             ${CMAKE_CURRENT_SOURCE_DIR}
                             ${ZLIB_INCLUDE_DIR}
                             ${Pangolin_INCLUDE_DIRS}
-                            ${CUDA_INCLUDE_DIRS}
+                            ${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES}
                             ${OPENNI2_INCLUDE_DIR}
                             ${OpenCV_INCLUDE_DIRS}
                             ${BOOST_INCLUDE_DIRS}
@@ -138,6 +148,12 @@ if(ROSNODE)
         ${eigen_conversions_LIBRARIES}
         ${cob_srvs_LIBRARIES}
     )
+if(HASROS2)
+    target_link_libraries(MultiMotionFusionTools
+        image_geometry::image_geometry
+        image_transport::image_transport
+    )
+endif()
     # to find "gSLICr.h" and "super_point_inference.hpp"
     target_link_libraries(MultiMotionFusionTools gSLICr_lib super_point_inference)
     add_definitions(-DROSNODE)
@@ -154,6 +170,12 @@ if(ROSBAG OR ROSNODE)
         ${tf2_eigen_LIBRARIES}
         ${image_geometry_LIBRARIES}
     )
+if(HASROS2)
+    target_link_libraries(MultiMotionFusionTools
+        image_geometry::image_geometry
+        cv_bridge::cv_bridge
+    )
+endif()
     add_definitions(-DROSCOMMON)
 endif()
 

diff --git a/GUI/MainController.cpp b/GUI/MainController.cpp
@@ -24,8 +24,10 @@
 #ifdef ROSBAG
 #include "Tools/RosBagReader.hpp"
 #endif
-#ifdef ROSNODE
+#ifdef ROSREADER
 #include "Tools/RosNodeReader.hpp"
+#endif
+#ifdef ROSSTATE
 #include "Tools/RosStatePublisher.hpp"
 #endif
 
@@ -244,16 +246,53 @@ MainController::MainController(int argc, char* argv[])
 #ifdef ROSNODE
   if (Parse::get().arg(argc, argv, "-ros", empty) > 0) {
     // instantiate MultiMotionFusion node
+#ifdef ROS1
     ros::init(argc, argv, "MMF");
+    executor = std::make_unique<ros::AsyncSpinner>(1);
+    executor->start();
+#elif defined(ROS2)
+    const std::vector<std::string> args_non_ros = rclcpp::init_and_remove_ros_arguments(argc, argv);
+    // reset argv without the parsed ROS args
+    for (int i = 0; i < argc; i++) {
+        memset(argv[i], 0, strlen(argv[i]));
+    }
+    argc = args_non_ros.size();
+    for (size_t i=0; i<args_non_ros.size(); i++) {
+      strcpy(argv[i], args_non_ros[i].c_str());
+    }
+
+    executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>(rclcpp::ExecutorOptions{}, 1);
+    spinner = std::thread([this](){ executor->spin(); });
+#endif
+#ifdef ROSREADER
     // read RGB-D data
     if (!logReader) {
-      logReader = std::make_unique<RosNodeReader>(15, Parse::get().arg(argc, argv, "-f", empty) > -1, target_dim);
-      logReaderReady = true;
+      try {
+        logReader = std::make_unique<RosNodeReader>(15, Parse::get().arg(argc, argv, "-f", empty) > -1, target_dim);
+        logReaderReady = true;
+      } catch (const std::runtime_error& e) {
+        std::cerr << "cannot create ROS RGB-D reader: " << e.what() << std::endl;
+        logReader = nullptr;
+        logReaderReady = false;
+      }
     }
+#endif
+#ifdef ROSSTATE
     // publish segmentation and point clouds
     // TODO: get camera frame from input images
     state_publisher = std::make_unique<RosStatePublisher>("rgb_camera_link");
+#endif
+#ifdef ROSUI
     ui_control = std::make_unique<RosInterface>(&gui, &mmf);
+#endif
+#if defined(ROS2)
+    // add nodes to executor
+#ifdef ROSREADER
+    if (logReader) {
+      executor->add_node(dynamic_cast<RosNodeReader*>(logReader.get())->n);
+    }
+#endif
+#endif
   }
 #endif
 
@@ -403,6 +442,14 @@ MainController::MainController(int argc, char* argv[])
 }
 
 MainController::~MainController() {
+#ifdef ROSNODE
+#ifdef ROS1
+  executor->stop();
+#elif defined(ROS2)
+  executor->cancel();
+  spinner.join();
+#endif
+#endif
   if (mmf) {
     delete mmf;
   }
@@ -455,7 +502,7 @@ void MainController::launch() {
         cudaCheckError();
       }
 
-#ifdef ROSNODE
+#ifdef ROSSTATE
     if (state_publisher) {
       state_publisher->reset();
     }
@@ -481,7 +528,7 @@ void MainController::launch() {
       //    gui->addModel(model->getID(), model->getConfidenceThreshold());}
       //);
 
-#ifdef ROSNODE
+#ifdef ROSSTATE
     if (state_publisher) {
       MultiMotionFusion::StatusMessageHandler send_status_message = std::bind(&RosStatePublisher::send_status_message, state_publisher.get(), std::placeholders::_1);
       mmf->setStatusMessageHandler(send_status_message);
@@ -649,7 +696,7 @@ void MainController::run() {
       gui->saveColorImage(viewPath);
     }
 
-#ifdef ROSNODE
+#ifdef ROSSTATE
     if (state_publisher) {
       const int64_t time = logReader->getFrameData().timestamp;
       state_publisher->pub_segmentation(mmf->getTextures()[GPUTexture::MASK_COLOR]->downloadTexture(), time);

diff --git a/GUI/MainController.h b/GUI/MainController.h
@@ -25,10 +25,16 @@
 #include "Tools/GroundTruthOdometry.h"
 #include "Tools/LogReader.h"
 
-#ifdef ROSNODE
+#ifdef ROSSTATE
 #include "Tools/RosStatePublisher.hpp"
+#endif
+#ifdef ROSUI
 #include "Tools/RosInterface.hpp"
 #endif
+#if defined(ROS2)
+#include <rclcpp/executors/multi_threaded_executor.hpp>
+#include <thread>
+#endif
 
 class MainController {
  public:
@@ -66,11 +72,22 @@ class MainController {
   bool exportModels;
   bool restore;
 
-#ifdef ROSNODE
+#ifdef ROSSTATE
   std::unique_ptr<RosStatePublisher> state_publisher;
+#endif
+#ifdef ROSUI
   std::unique_ptr<RosInterface> ui_control;
 #endif
 
+#ifdef ROSNODE
+#ifdef ROS1
+  std::unique_ptr<ros::AsyncSpinner> executor;
+#elif defined(ROS2)
+  std::unique_ptr<rclcpp::executors::MultiThreadedExecutor> executor;
+  std::thread spinner;
+#endif
+#endif
+
   float confGlobalInit, confObjectInit, icpErrThresh, covThresh, photoThresh, fernThresh;
 
   int timeDelta, icpCountThresh, start, end, preallocatedModelsCount;

diff --git a/GUI/Tools/RosInterface.cpp b/GUI/Tools/RosInterface.cpp
@@ -1,4 +1,4 @@
-#ifdef ROSNODE
+#ifdef ROSUI
 
 #include "RosInterface.hpp"