ros2: move package path lookup out of the shared core

mins/cmake/ROS1.cmake

@@ -51,6 +51,7 @@ list(APPEND LIBRARY_SOURCES
         src/sim/Simulator.cpp
         src/sim/ConstBsplineSE3.cpp
         src/sim/SimVisualizer.cpp
+        src/utils/PackagePath.cpp
         src/utils/Print_Logger.cpp
         src/utils/Jabdongsani.cpp
         src/state/State.cpp

mins/cmake/ROS2.cmake

@@ -84,6 +84,7 @@ list(APPEND LIBRARY_SOURCES
         src/sim/Simulator.cpp
         src/sim/ConstBsplineSE3.cpp
         src/sim/Sim2Visualizer.cpp
+        src/utils/PackagePath.cpp
         src/utils/Print_Logger.cpp
         src/utils/Jabdongsani.cpp
         src/state/State.cpp

mins/src/options/OptionsSimulation.cpp

@@ -27,13 +27,9 @@
 
 #include "OptionsSimulation.h"
 #include "OptionsEstimator.h"
+#include "utils/PackagePath.h"
 #include "utils/Print_Logger.h"
 #include "utils/opencv_yaml_parse.h"
-#if ROS_AVAILABLE == 1
-#include <ros/package.h>
-#elif ROS_AVAILABLE == 2
-#include <ament_index_cpp/get_package_share_directory.hpp>
-#endif
 
 mins::OptionsSimulation::OptionsSimulation() { est_true = std::make_shared<OptionsEstimator>(); }
 
@@ -65,12 +61,7 @@ void mins::OptionsSimulation::load_print(const std::shared_ptr<ov_core::YamlPars
     WtoE_trans.block(0, 0, 4, 1) = ov_core::rot_2_quat(T.block(0, 0, 3, 3));
     WtoE_trans.block(4, 0, 3, 1) = T.block(0, 3, 3, 1);
     // Replace MINS_DATA_DIR if we have it
-
-#if ROS_AVAILABLE == 1
-    auto dir = ros::package::getPath("mins_data");
-#elif ROS_AVAILABLE == 2
-    auto dir = ament_index_cpp::get_package_share_directory("mins_data");
-#endif
+    auto dir = mins::get_package_path("mins_data");
     BSpline_path.substr(0, 13) == "MINS_DATA_DIR" ? BSpline_path.replace(0, 13, dir) : std::string();
     planes_path.substr(0, 13) == "MINS_DATA_DIR" ? planes_path.replace(0, 13, dir) : std::string();
 

mins/src/options/OptionsSystem.cpp

@@ -19,13 +19,9 @@
  */
 
 #include "OptionsSystem.h"
+#include "utils/PackagePath.h"
 #include "utils/Print_Logger.h"
 #include "utils/opencv_yaml_parse.h"
-#if ROS_AVAILABLE == 1
-#include <ros/package.h>
-#elif ROS_AVAILABLE == 2
-#include <ament_index_cpp/get_package_share_directory.hpp>
-#endif
 
 void mins::OptionsSystem::load_print(const std::shared_ptr<ov_core::YamlParser> &parser) {
   if (parser != nullptr) {
@@ -46,11 +42,7 @@ void mins::OptionsSystem::load_print(const std::shared_ptr<ov_core::YamlParser>
     parser->parse_external(f, "sys", "bag_durr", bag_durr);
     parser->parse_external(f, "sys", "path_gt", path_gt, false);
     // Replace MINS_DIR if we have it
-#if ROS_AVAILABLE == 1
-    auto dir = ros::package::getPath("mins");
-#elif ROS_AVAILABLE == 2
-    auto dir = ament_index_cpp::get_package_share_directory("mins");
-#endif
+    auto dir = mins::get_package_path("mins");
     path_timing.substr(0, 8) == "MINS_DIR" ? path_timing.replace(0, 8, dir) : std::string();
     path_state.substr(0, 8) == "MINS_DIR" ? path_state.replace(0, 8, dir) : std::string();
     path_trajectory.substr(0, 8) == "MINS_DIR" ? path_trajectory.replace(0, 8, dir) : std::string();

mins/src/utils/PackagePath.cpp

@@ -0,0 +1,38 @@
+/*
+ * MINS: Efficient and Robust Multisensor-aided Inertial Navigation System
+ * Copyright (C) 2023 Woosik Lee
+ * Copyright (C) 2023 Guoquan Huang
+ * Copyright (C) 2023 MINS Contributors
+ *
+ * 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 <https://www.gnu.org/licenses/>.
+ */
+
+// This is the only translation unit that touches the ROS package-path API,
+// keeping ros::package / ament out of the shared core.
+#include "utils/PackagePath.h"
+#if ROS_AVAILABLE == 1
+#include <ros/package.h>
+#elif ROS_AVAILABLE == 2
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#endif
+
+std::string mins::get_package_path(const std::string &pkg) {
+#if ROS_AVAILABLE == 1
+  return ros::package::getPath(pkg);
+#elif ROS_AVAILABLE == 2
+  return ament_index_cpp::get_package_share_directory(pkg);
+#else
+  return std::string();
+#endif
+}

mins/src/utils/PackagePath.h

@@ -0,0 +1,32 @@
+/*
+ * MINS: Efficient and Robust Multisensor-aided Inertial Navigation System
+ * Copyright (C) 2023 Woosik Lee
+ * Copyright (C) 2023 Guoquan Huang
+ * Copyright (C) 2023 MINS Contributors
+ *
+ * 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 <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef MINS_PACKAGE_PATH_H
+#define MINS_PACKAGE_PATH_H
+
+#include <string>
+
+namespace mins {
+// Resolve an installed package's directory. Defined at the ROS boundary
+// (PackagePath.cpp) so the shared core stays free of ros::package / ament.
+std::string get_package_path(const std::string &pkg);
+} // namespace mins
+
+#endif // MINS_PACKAGE_PATH_H