Add component stella_vslam_ros::System (#156)

CMakeLists.txt

@@ -118,6 +118,7 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(image_transport REQUIRED)
 find_package(message_filters REQUIRED)
@@ -191,4 +192,48 @@ endif()
 
 add_subdirectory(src)
 
+add_library(stella_vslam_ros_system SHARED
+  src/system.cc src/stella_vslam_ros.cc
+)
+# TODO: Move heder files to include directory
+target_include_directories(stella_vslam_ros_system PRIVATE
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>
+  $<INSTALL_INTERFACE:src>)
+rclcpp_components_register_node(stella_vslam_ros_system PLUGIN "stella_vslam_ros::System" EXECUTABLE system)
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_target_dependencies(stella_vslam_ros_system
+  PUBLIC
+  rclcpp
+  rclcpp_components
+  cv_bridge
+  image_transport
+  message_filters
+  rcutils
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  tf2
+  tf2_eigen
+  tf2_geometry_msgs
+  tf2_msgs
+  tf2_ros)
+target_link_libraries(stella_vslam_ros_system PRIVATE
+  yaml-cpp
+  stella_vslam::stella_vslam)
+if(spdlog_FOUND)
+  target_link_libraries(stella_vslam_ros_system PRIVATE
+    spdlog::spdlog)
+endif()
+if(NOT WIN32)
+  ament_environment_hooks(
+    "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}")
+endif()
+install(TARGETS
+  stella_vslam_ros_system
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
 ament_package()

package.xml

@@ -18,6 +18,8 @@
     <buildtool_depend>ament_cmake</buildtool_depend>
 
     <depend>stella_vslam</depend>
+    <depend>rclcpp</depend>
+    <depend>rclcpp_components</depend>
     <depend>eigen</depend>
     <depend>libgoogle-glog-dev</depend>
     <depend>image_transport</depend>

src/system.cc

@@ -0,0 +1,106 @@
+#include <stella_vslam/system.h>
+#include <stella_vslam/config.h>
+#include <stella_vslam/util/yaml.h>
+#include <stella_vslam_ros.h>
+
+#include <iostream>
+#include <chrono>
+#include <fstream>
+#include <numeric>
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <spdlog/spdlog.h>
+
+#include <ghc/filesystem.hpp>
+namespace fs = ghc::filesystem;
+
+namespace stella_vslam_ros {
+
+class System : public rclcpp::Node {
+public:
+    System(
+        const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+    System(
+        const std::string& name_space,
+        const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
+    std::shared_ptr<stella_vslam_ros::system> slam_ros_;
+    std::shared_ptr<stella_vslam::system> slam_;
+    std::shared_ptr<stella_vslam::config> cfg_;
+};
+
+System::System(
+    const rclcpp::NodeOptions& options)
+    : System("", options) {}
+
+System::System(
+    const std::string& name_space,
+    const rclcpp::NodeOptions& options)
+    : Node("run_slam", name_space, options) {
+    std::string vocab_file_path = declare_parameter("vocab_file_path", "");
+    std::string setting_file_path = declare_parameter("setting_file_path", "");
+    std::string log_level = declare_parameter("log_level", "info");
+    std::string map_db_path_in = declare_parameter("map_db_path_in", "");
+    std::string map_db_path_out = declare_parameter("map_db_path_out", "");
+    bool disable_mapping = declare_parameter("disable_mapping", false);
+    bool temporal_mapping = declare_parameter("temporal_mapping", false);
+
+    if (vocab_file_path.empty() || setting_file_path.empty()) {
+        RCLCPP_FATAL(get_logger(), "Invalid parameter");
+        return;
+    }
+
+    // setup logger
+    spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] %^[%L] %v%$");
+    spdlog::set_level(spdlog::level::from_str(log_level));
+
+    // load configuration
+    try {
+        cfg_ = std::make_shared<stella_vslam::config>(setting_file_path);
+    }
+    catch (const std::exception& e) {
+        RCLCPP_FATAL(get_logger(), e.what());
+        return;
+    }
+
+    slam_ = std::make_shared<stella_vslam::system>(cfg_, vocab_file_path);
+    bool need_initialize = true;
+    if (!map_db_path_in.empty()) {
+        need_initialize = false;
+        const auto path = fs::path(map_db_path_in);
+        if (path.extension() == ".yaml") {
+            YAML::Node node = YAML::LoadFile(path);
+            for (const auto& map_path : node["maps"].as<std::vector<std::string>>()) {
+                slam_->load_map_database(path.parent_path() / map_path);
+            }
+        }
+        else {
+            // load the prebuilt map
+            slam_->load_map_database(path);
+        }
+    }
+    slam_->startup(need_initialize);
+    if (disable_mapping) {
+        slam_->disable_mapping_module();
+    }
+    else if (temporal_mapping) {
+        slam_->enable_temporal_mapping();
+        slam_->disable_loop_detector();
+    }
+
+    if (slam_->get_camera()->setup_type_ == stella_vslam::camera::setup_type_t::Monocular) {
+        slam_ros_ = std::make_shared<stella_vslam_ros::mono>(slam_, "");
+    }
+    else {
+        RCLCPP_FATAL_STREAM(get_logger(), "Invalid setup type: " << slam_->get_camera()->get_setup_type_string());
+        return;
+    }
+}
+
+} // namespace stella_vslam_ros
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(stella_vslam_ros::System)