From 5d9f83eca05b2960877dee173a8d9f20271c7851 Mon Sep 17 00:00:00 2001 From: ymd-stella <7959916+ymd-stella@users.noreply.github.com> Date: Sat, 30 Mar 2024 19:05:18 +0900 Subject: [PATCH] Use iridescence_viewer (#177) --- CMakeLists.txt | 14 +++++++++++ src/CMakeLists.txt | 15 +++++------ src/run_slam.cc | 55 ++++++++++++++++++++++++++++++++++++++--- src/run_slam_offline.cc | 55 ++++++++++++++++++++++++++++++++++++++--- src/system.cc | 53 +++++++++++++++++++++++++++++++++++++-- 5 files changed, 177 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f6d415..2a6d2ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -185,6 +185,11 @@ find_package(socket_publisher QUIET) if(socket_publisher_FOUND) message(STATUS "Viewer for examples: SocketPublisher") endif() +find_package(iridescence_viewer QUIET) +if(iridescence_viewer_FOUND) + message(STATUS "Viewer for examples: IridescenceViewer") +endif() + ######### # Build # @@ -211,6 +216,15 @@ if(socket_publisher_FOUND) target_link_libraries(stella_vslam_ros_system PRIVATE socket_publisher::socket_publisher ${SIOCLIENT_LIBRARY} ${PROTOBUF_LIBRARIES}) endif() +if(iridescence_viewer_FOUND) + # Set macro flag + target_compile_definitions(stella_vslam_ros_system + PRIVATE HAVE_IRIDESCENCE_VIEWER) + # Link viewer + target_link_libraries(stella_vslam_ros_system PRIVATE + iridescence_viewer::iridescence_viewer) +endif() + # TODO: Move heder files to include directory target_include_directories(stella_vslam_ros_system PRIVATE $ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 75f74f8..4eeee85 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,10 +1,3 @@ -if(pangolin_viewer_FOUND) - message(STATUS "Viewer for examples: pangolin viewer") -endif() -if(socket_publisher_FOUND) - message(STATUS "Viewer for examples: socket publisher") -endif() - set(EXECUTABLE_TARGETS "") include_directories(${CMAKE_CURRENT_SOURCE_DIR}) @@ -30,6 +23,14 @@ foreach(EXECUTABLE_TARGET IN LISTS EXECUTABLE_TARGETS) target_link_libraries(${EXECUTABLE_TARGET} PRIVATE socket_publisher::socket_publisher ${SIOCLIENT_LIBRARY} ${PROTOBUF_LIBRARIES}) endif() + if(iridescence_viewer_FOUND) + # Set macro flag + target_compile_definitions(${EXECUTABLE_TARGET} + PRIVATE HAVE_IRIDESCENCE_VIEWER) + # Link viewer + target_link_libraries(${EXECUTABLE_TARGET} PRIVATE + iridescence_viewer::iridescence_viewer) + endif() # setup stack trace logger if(USE_STACK_TRACE_LOGGER) diff --git a/src/run_slam.cc b/src/run_slam.cc index dcda8a1..51efc14 100644 --- a/src/run_slam.cc +++ b/src/run_slam.cc @@ -1,6 +1,9 @@ #ifdef HAVE_PANGOLIN_VIEWER #include "pangolin_viewer/viewer.h" #endif +#ifdef HAVE_IRIDESCENCE_VIEWER +#include "iridescence_viewer/viewer.h" +#endif #ifdef HAVE_SOCKET_PUBLISHER #include "socket_publisher/publisher.h" #endif @@ -51,6 +54,29 @@ void tracking(const std::shared_ptr& slam_ros, SLAM->get_map_publisher()); } #endif +#ifdef HAVE_IRIDESCENCE_VIEWER + std::shared_ptr iridescence_viewer; + std::mutex mtx_terminate; + bool terminate_is_requested = false; + if (viewer_string == "iridescence_viewer") { + iridescence_viewer = std::make_shared( + stella_vslam::util::yaml_optional_ref(cfg->yaml_node_, "IridescenceViewer"), + SLAM->get_frame_publisher(), + SLAM->get_map_publisher()); + iridescence_viewer->add_button("Reset", [&SLAM] { + SLAM->request_reset(); + }); + iridescence_viewer->add_button("Save and exit", [&terminate_is_requested, &mtx_terminate, &iridescence_viewer] { + std::lock_guard lock(mtx_terminate); + terminate_is_requested = true; + iridescence_viewer->request_terminate(); + }); + iridescence_viewer->add_close_callback([&terminate_is_requested, &mtx_terminate] { + std::lock_guard lock(mtx_terminate); + terminate_is_requested = true; + }); + } +#endif #ifdef HAVE_SOCKET_PUBLISHER std::shared_ptr publisher; if (viewer_string == "socket_publisher") { @@ -70,6 +96,11 @@ void tracking(const std::shared_ptr& slam_ros, if (viewer_string == "pangolin_viewer") { #ifdef HAVE_PANGOLIN_VIEWER viewer->run(); +#endif + } + if (viewer_string == "iridescence_viewer") { +#ifdef HAVE_IRIDESCENCE_VIEWER + iridescence_viewer->run(); #endif } if (viewer_string == "socket_publisher") { @@ -93,6 +124,11 @@ void tracking(const std::shared_ptr& slam_ros, if (viewer_string == "pangolin_viewer") { #ifdef HAVE_PANGOLIN_VIEWER viewer->request_terminate(); +#endif + } + if (viewer_string == "iridescence_viewer") { +#ifdef HAVE_IRIDESCENCE_VIEWER + iridescence_viewer->request_terminate(); #endif } if (viewer_string == "socket_publisher") { @@ -155,7 +191,7 @@ int main(int argc, char* argv[]) { auto disable_mapping = op.add("", "disable-mapping", "disable mapping"); auto temporal_mapping = op.add("", "temporal-mapping", "enable temporal mapping"); auto rectify = op.add("r", "rectify", "rectify stereo image"); - auto viewer = op.add>("", "viewer", "viewer [pangolin_viewer, socket_publisher, none]"); + auto viewer = op.add>("", "viewer", "viewer [iridescence_viewer, pangolin_viewer, socket_publisher, none]"); try { op.parse(argc, argv); } @@ -182,7 +218,10 @@ int main(int argc, char* argv[]) { std::string viewer_string; if (viewer->is_set()) { viewer_string = viewer->value(); - if (viewer_string != "pangolin_viewer" && viewer_string != "socket_publisher" && viewer_string != "none") { + if (viewer_string != "pangolin_viewer" + && viewer_string != "socket_publisher" + && viewer_string != "iridescence_viewer" + && viewer_string != "none") { std::cerr << "invalid arguments (--viewer)" << std::endl << std::endl << op << std::endl; @@ -196,6 +235,14 @@ int main(int argc, char* argv[]) { return EXIT_FAILURE; } #endif +#ifndef HAVE_IRIDESCENCE_VIEWER + if (viewer_string == "iridescence_viewer") { + std::cerr << "iridescence_viewer not linked" << std::endl + << std::endl + << op << std::endl; + return EXIT_FAILURE; + } +#endif #ifndef HAVE_SOCKET_PUBLISHER if (viewer_string == "socket_publisher") { std::cerr << "socket_publisher not linked" << std::endl @@ -206,7 +253,9 @@ int main(int argc, char* argv[]) { #endif } else { -#ifdef HAVE_PANGOLIN_VIEWER +#ifdef HAVE_IRIDESCENCE_VIEWER + viewer_string = "iridescence_viewer"; +#elif defined(HAVE_PANGOLIN_VIEWER) viewer_string = "pangolin_viewer"; #elif defined(HAVE_SOCKET_PUBLISHER) viewer_string = "socket_publisher"; diff --git a/src/run_slam_offline.cc b/src/run_slam_offline.cc index 2bf2385..2521189 100644 --- a/src/run_slam_offline.cc +++ b/src/run_slam_offline.cc @@ -1,6 +1,9 @@ #ifdef HAVE_PANGOLIN_VIEWER #include "pangolin_viewer/viewer.h" #endif +#ifdef HAVE_IRIDESCENCE_VIEWER +#include "iridescence_viewer/viewer.h" +#endif #ifdef HAVE_SOCKET_PUBLISHER #include "socket_publisher/publisher.h" #endif @@ -66,6 +69,29 @@ void tracking(const std::shared_ptr& slam_ros, SLAM->get_map_publisher()); } #endif +#ifdef HAVE_IRIDESCENCE_VIEWER + std::shared_ptr iridescence_viewer; + std::mutex mtx_terminate; + bool terminate_is_requested = false; + if (viewer_string == "iridescence_viewer") { + iridescence_viewer = std::make_shared( + stella_vslam::util::yaml_optional_ref(cfg->yaml_node_, "IridescenceViewer"), + SLAM->get_frame_publisher(), + SLAM->get_map_publisher()); + iridescence_viewer->add_button("Reset", [&SLAM] { + SLAM->request_reset(); + }); + iridescence_viewer->add_button("Save and exit", [&terminate_is_requested, &mtx_terminate, &iridescence_viewer] { + std::lock_guard lock(mtx_terminate); + terminate_is_requested = true; + iridescence_viewer->request_terminate(); + }); + iridescence_viewer->add_close_callback([&terminate_is_requested, &mtx_terminate] { + std::lock_guard lock(mtx_terminate); + terminate_is_requested = true; + }); + } +#endif #ifdef HAVE_SOCKET_PUBLISHER std::shared_ptr publisher; if (viewer_string == "socket_publisher") { @@ -85,6 +111,11 @@ void tracking(const std::shared_ptr& slam_ros, if (viewer_string == "pangolin_viewer") { #ifdef HAVE_PANGOLIN_VIEWER viewer->run(); +#endif + } + if (viewer_string == "iridescence_viewer") { +#ifdef HAVE_IRIDESCENCE_VIEWER + iridescence_viewer->run(); #endif } if (viewer_string == "socket_publisher") { @@ -238,6 +269,11 @@ void tracking(const std::shared_ptr& slam_ros, if (viewer_string == "pangolin_viewer") { #ifdef HAVE_PANGOLIN_VIEWER viewer->request_terminate(); +#endif + } + if (viewer_string == "iridescence_viewer") { +#ifdef HAVE_IRIDESCENCE_VIEWER + iridescence_viewer->request_terminate(); #endif } if (viewer_string == "socket_publisher") { @@ -309,7 +345,7 @@ int main(int argc, char* argv[]) { auto disable_mapping = op.add("", "disable-mapping", "disable mapping"); auto temporal_mapping = op.add("", "temporal-mapping", "enable temporal mapping"); auto rectify = op.add("r", "rectify", "rectify stereo image"); - auto viewer = op.add>("", "viewer", "viewer [pangolin_viewer, socket_publisher, none]"); + auto viewer = op.add>("", "viewer", "viewer [iridescence_viewer, pangolin_viewer, socket_publisher, none]"); try { op.parse(argc, argv); } @@ -336,7 +372,10 @@ int main(int argc, char* argv[]) { std::string viewer_string; if (viewer->is_set()) { viewer_string = viewer->value(); - if (viewer_string != "pangolin_viewer" && viewer_string != "socket_publisher" && viewer_string != "none") { + if (viewer_string != "pangolin_viewer" + && viewer_string != "socket_publisher" + && viewer_string != "iridescence_viewer" + && viewer_string != "none") { std::cerr << "invalid arguments (--viewer)" << std::endl << std::endl << op << std::endl; @@ -350,6 +389,14 @@ int main(int argc, char* argv[]) { return EXIT_FAILURE; } #endif +#ifndef HAVE_IRIDESCENCE_VIEWER + if (viewer_string == "iridescence_viewer") { + std::cerr << "iridescence_viewer not linked" << std::endl + << std::endl + << op << std::endl; + return EXIT_FAILURE; + } +#endif #ifndef HAVE_SOCKET_PUBLISHER if (viewer_string == "socket_publisher") { std::cerr << "socket_publisher not linked" << std::endl @@ -360,7 +407,9 @@ int main(int argc, char* argv[]) { #endif } else { -#ifdef HAVE_PANGOLIN_VIEWER +#ifdef HAVE_IRIDESCENCE_VIEWER + viewer_string = "iridescence_viewer"; +#elif defined(HAVE_PANGOLIN_VIEWER) viewer_string = "pangolin_viewer"; #elif defined(HAVE_SOCKET_PUBLISHER) viewer_string = "socket_publisher"; diff --git a/src/system.cc b/src/system.cc index fedb62b..5c63e22 100644 --- a/src/system.cc +++ b/src/system.cc @@ -1,6 +1,9 @@ #ifdef HAVE_PANGOLIN_VIEWER #include "pangolin_viewer/viewer.h" #endif +#ifdef HAVE_IRIDESCENCE_VIEWER +#include "iridescence_viewer/viewer.h" +#endif #ifdef HAVE_SOCKET_PUBLISHER #include "socket_publisher/publisher.h" #endif @@ -43,6 +46,9 @@ class System : public rclcpp::Node { #ifdef HAVE_PANGOLIN_VIEWER std::shared_ptr viewer_; #endif +#ifdef HAVE_IRIDESCENCE_VIEWER + std::shared_ptr iridescence_viewer_; +#endif #ifdef HAVE_SOCKET_PUBLISHER std::shared_ptr publisher_; #endif @@ -73,7 +79,10 @@ System::System( // viewer if (!viewer.empty()) { viewer_string_ = viewer; - if (viewer_string_ != "pangolin_viewer" && viewer_string_ != "socket_publisher" && viewer_string_ != "none") { + if (viewer_string_ != "pangolin_viewer" + && viewer_string_ != "socket_publisher" + && viewer_string_ != "iridescence_viewer" + && viewer_string_ != "none") { RCLCPP_FATAL(get_logger(), "invalid arguments (--viewer)"); return; } @@ -83,6 +92,12 @@ System::System( return; } #endif +#ifndef HAVE_IRIDESCENCE_VIEWER + if (viewer_string_ == "iridescence_viewer") { + RCLCPP_FATAL(get_logger(), "iridescence_viewer not linked"); + return; + } +#endif #ifndef HAVE_SOCKET_PUBLISHER if (viewer_string_ == "socket_publisher") { RCLCPP_FATAL(get_logger(), "socket_publisher not linked"); @@ -91,7 +106,9 @@ System::System( #endif } else { -#ifdef HAVE_PANGOLIN_VIEWER +#ifdef HAVE_IRIDESCENCE_VIEWER + viewer_string_ = "iridescence_viewer"; +#elif defined(HAVE_PANGOLIN_VIEWER) viewer_string_ = "pangolin_viewer"; #elif defined(HAVE_SOCKET_PUBLISHER) viewer_string_ = "socket_publisher"; @@ -155,6 +172,28 @@ System::System( slam_->get_map_publisher()); } #endif +#ifdef HAVE_IRIDESCENCE_VIEWER + std::mutex mtx_terminate; + bool terminate_is_requested = false; + if (viewer_string_ == "iridescence_viewer") { + iridescence_viewer_ = std::make_shared( + stella_vslam::util::yaml_optional_ref(cfg->yaml_node_, "IridescenceViewer"), + SLAM->get_frame_publisher(), + SLAM->get_map_publisher()); + iridescence_viewer_->add_button("Reset", [&SLAM] { + SLAM->request_reset(); + }); + iridescence_viewer_->add_button("Save and exit", [&terminate_is_requested, &mtx_terminate, &iridescence_viewer] { + std::lock_guard lock(mtx_terminate); + terminate_is_requested = true; + iridescence_viewer->request_terminate(); + }); + iridescence_viewer_->add_close_callback([&terminate_is_requested, &mtx_terminate] { + std::lock_guard lock(mtx_terminate); + terminate_is_requested = true; + }); + } +#endif #ifdef HAVE_SOCKET_PUBLISHER if (viewer_string_ == "socket_publisher") { publisher_ = std::make_shared( @@ -172,6 +211,11 @@ System::System( if (viewer_string_ == "pangolin_viewer") { #ifdef HAVE_PANGOLIN_VIEWER viewer_->run(); +#endif + } + if (viewer_string_ == "iridescence_viewer") { +#ifdef HAVE_IRIDESCENCE_VIEWER + iridescence_viewer_->run(); #endif } if (viewer_string_ == "socket_publisher") { @@ -195,6 +239,11 @@ System::~System() { if (viewer_string_ == "pangolin_viewer") { #ifdef HAVE_PANGOLIN_VIEWER viewer_->request_terminate(); +#endif + } + if (viewer_string_ == "iridescence_viewer") { +#ifdef HAVE_IRIDESCENCE_VIEWER + iridescence_viewer_->request_terminate(); #endif } if (viewer_string_ == "socket_publisher") {