Enable gui in stella_vslam_ros::System

stella-cv · Sep 3, 2023 · df0e5fb · df0e5fb
1 parent 1f89b18
commit df0e5fb
Showing 1 changed file with 110 additions and 1 deletion.
diff --git a/src/system.cc b/src/system.cc
@@ -25,10 +25,20 @@ class System : public rclcpp::Node {
     System(
         const std::string& name_space,
         const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+    virtual ~System();
 
     std::shared_ptr<stella_vslam_ros::system> slam_ros_;
     std::shared_ptr<stella_vslam::system> slam_;
     std::shared_ptr<stella_vslam::config> cfg_;
+    std::string map_db_path_out_;
+    std::string viewer_string_;
+    std::shared_ptr<std::thread> viewer_thread_;
+#ifdef HAVE_PANGOLIN_VIEWER
+    std::shared_ptr<pangolin_viewer::viewer> viewer_;
+#endif
+#ifdef HAVE_SOCKET_PUBLISHER
+    std::shared_ptr<socket_publisher::publisher> publisher_;
+#endif
 };
 
 System::System(
@@ -43,15 +53,44 @@ System::System(
     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", "");
+    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);
+    std::string viewer = declare_parameter("viewer", "none");
 
     if (vocab_file_path.empty() || setting_file_path.empty()) {
         RCLCPP_FATAL(get_logger(), "Invalid parameter");
         return;
     }
 
+    // viewer
+    if (!viewer.empty()) {
+        viewer_string_ = viewer;
+        if (viewer_string_ != "pangolin_viewer" && viewer_string_ != "socket_publisher" && viewer_string_ != "none") {
+            RCLCPP_FATAL(get_logger(), "invalid arguments (--viewer)");
+            return;
+        }
+#ifndef HAVE_PANGOLIN_VIEWER
+        if (viewer_string_ == "pangolin_viewer") {
+            RCLCPP_FATAL(get_logger(), "pangolin_viewer not linked");
+            return;
+        }
+#endif
+#ifndef HAVE_SOCKET_PUBLISHER
+        if (viewer_string_ == "socket_publisher") {
+            RCLCPP_FATAL(get_logger(), "socket_publisher not linked");
+            return;
+        }
+#endif
+    }
+    else {
+#ifdef HAVE_PANGOLIN_VIEWER
+        viewer_string_ = "pangolin_viewer";
+#elif defined(HAVE_SOCKET_PUBLISHER)
+        viewer_string_ = "socket_publisher";
+#endif
+    }
+
     // setup logger
     spdlog::set_pattern("[%Y-%m-%d %H:%M:%S.%e] %^[%L] %v%$");
     spdlog::set_level(spdlog::level::from_str(log_level));
@@ -97,6 +136,76 @@ System::System(
         RCLCPP_FATAL_STREAM(get_logger(), "Invalid setup type: " << slam_->get_camera()->get_setup_type_string());
         return;
     }
+
+    // create a viewer object
+    // and pass the frame_publisher and the map_publisher
+#ifdef HAVE_PANGOLIN_VIEWER
+    if (viewer_string_ == "pangolin_viewer") {
+        viewer_ = std::make_shared<pangolin_viewer::viewer>(
+            stella_vslam::util::yaml_optional_ref(cfg->yaml_node_, "PangolinViewer"),
+            slam_,
+            slam_->get_frame_publisher(),
+            slam_->get_map_publisher());
+    }
+#endif
+#ifdef HAVE_SOCKET_PUBLISHER
+    if (viewer_string_ == "socket_publisher") {
+        publisher_ = std::make_shared<socket_publisher::publisher>(
+            stella_vslam::util::yaml_optional_ref(cfg->yaml_node_, "SocketPublisher"),
+            slam_,
+            slam_->get_frame_publisher(),
+            slam_->get_map_publisher());
+    }
+#endif
+
+    if (viewer_string_ != "none") {
+        // TODO: Pangolin needs to run in the main thread on OSX
+        // run the viewer in another thread
+        viewer_thread_ = std::make_shared<std::thread>([&]() {
+            if (viewer_string_ == "pangolin_viewer") {
+#ifdef HAVE_PANGOLIN_VIEWER
+                viewer->run();
+#endif
+            }
+            if (viewer_string_ == "socket_publisher") {
+#ifdef HAVE_SOCKET_PUBLISHER
+                publisher->run();
+#endif
+            }
+            if (slam_->terminate_is_requested()) {
+                // wait until the loop BA is finished
+                while (slam_->loop_BA_is_running()) {
+                    std::this_thread::sleep_for(std::chrono::microseconds(5000));
+                }
+                rclcpp::shutdown();
+            }
+        });
+    }
+}
+
+System::~System() {
+    // automatically close the viewer
+    if (viewer_string_ == "pangolin_viewer") {
+#ifdef HAVE_PANGOLIN_VIEWER
+        viewer_->request_terminate();
+#endif
+    }
+    if (viewer_string_ == "socket_publisher") {
+#ifdef HAVE_SOCKET_PUBLISHER
+        publisher_->request_terminate();
+#endif
+    }
+    if (viewer_string_ != "none") {
+        viewer_thread_->join();
+    }
+
+    // shutdown the SLAM process
+    slam_->shutdown();
+
+    if (!map_db_path_out_.empty()) {
+        // output the map database
+        slam_->save_map_database(map_db_path_out_);
+    }
 }
 
 } // namespace stella_vslam_ros