From cc6071ee15b2d6a1a065976d951f99300210a3e9 Mon Sep 17 00:00:00 2001 From: Mohamed Abdelkader Date: Mon, 20 May 2024 22:56:27 +0300 Subject: [PATCH] Add subscriber ROS1 node --- ov_plane/CMakeLists.txt | 8 ++ ov_plane/launch/subscribe.launch | 89 +++++++++++++++++++ ov_plane/src/run_subscribe_msckf.cpp | 125 +++++++++++++++++++++++++++ 3 files changed, 222 insertions(+) create mode 100644 ov_plane/launch/subscribe.launch create mode 100644 ov_plane/src/run_subscribe_msckf.cpp diff --git a/ov_plane/CMakeLists.txt b/ov_plane/CMakeLists.txt index 19928df..6be38ad 100644 --- a/ov_plane/CMakeLists.txt +++ b/ov_plane/CMakeLists.txt @@ -143,6 +143,14 @@ install(TARGETS timing_custom RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +add_executable(run_subscribe_msckf src/run_subscribe_msckf.cpp) +target_link_libraries(run_subscribe_msckf ov_plane_lib ${thirdparty_libraries}) +install(TARGETS run_subscribe_msckf + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + ################################################## # Launch files! ################################################## diff --git a/ov_plane/launch/subscribe.launch b/ov_plane/launch/subscribe.launch new file mode 100644 index 0000000..af95e6c --- /dev/null +++ b/ov_plane/launch/subscribe.launch @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ov_plane/src/run_subscribe_msckf.cpp b/ov_plane/src/run_subscribe_msckf.cpp new file mode 100644 index 0000000..41d90a3 --- /dev/null +++ b/ov_plane/src/run_subscribe_msckf.cpp @@ -0,0 +1,125 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2023 Patrick Geneva + * Copyright (C) 2018-2023 Guoquan Huang + * Copyright (C) 2018-2023 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +#include "core/VioManager.h" +#include "core/VioManagerOptions.h" +#include "utils/dataset_reader.h" + +#if ROS_AVAILABLE == 1 +#include "ros/ROS1Visualizer.h" +#include +#elif ROS_AVAILABLE == 2 +#include "ros/ROS2Visualizer.h" +#include +#endif + +using namespace ov_plane; + +std::shared_ptr sys; +#if ROS_AVAILABLE == 1 +std::shared_ptr viz; +#elif ROS_AVAILABLE == 2 +std::shared_ptr viz; +#endif + +// Main function +int main(int argc, char **argv) { + + // Ensure we have a path, if the user passes it then we should use it + std::string config_path = "unset_path_to_config.yaml"; + if (argc > 1) { + config_path = argv[1]; + } + +#if ROS_AVAILABLE == 1 + // Launch our ros node + ros::init(argc, argv, "run_subscribe_msckf"); + auto nh = std::make_shared("~"); + nh->param("config_path", config_path, config_path); +#elif ROS_AVAILABLE == 2 + // Launch our ros node + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + options.allow_undeclared_parameters(true); + options.automatically_declare_parameters_from_overrides(true); + auto node = std::make_shared("run_subscribe_msckf", options); + node->get_parameter("config_path", config_path); +#endif + + // Load the config + auto parser = std::make_shared(config_path); +#if ROS_AVAILABLE == 1 + parser->set_node_handler(nh); +#elif ROS_AVAILABLE == 2 + parser->set_node(node); +#endif + + // Verbosity + std::string verbosity = "DEBUG"; + parser->parse_config("verbosity", verbosity); + ov_core::Printer::setPrintLevel(verbosity); + + // Create our VIO system + VioManagerOptions params; + params.print_and_load(parser); + params.use_multi_threading_subs = true; + sys = std::make_shared(params); +#if ROS_AVAILABLE == 1 + viz = std::make_shared(nh, sys); + viz->setup_subscribers(parser); +#elif ROS_AVAILABLE == 2 + viz = std::make_shared(node, sys); + viz->setup_subscribers(parser); +#endif + + // Ensure we read in all parameters required + if (!parser->successful()) { + PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); + std::exit(EXIT_FAILURE); + } + + // Spin off to ROS + PRINT_DEBUG("done...spinning to ros\n"); +#if ROS_AVAILABLE == 1 + // ros::spin(); + ros::AsyncSpinner spinner(0); + spinner.start(); + ros::waitForShutdown(); +#elif ROS_AVAILABLE == 2 + // rclcpp::spin(node); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); +#endif + + // Final visualization + viz->visualize_final(); +#if ROS_AVAILABLE == 1 + ros::shutdown(); +#elif ROS_AVAILABLE == 2 + rclcpp::shutdown(); +#endif + + // Done! + return EXIT_SUCCESS; +}