Add a ROS 2 node for serial MSCKF processing of ROS 2 bags

ov_eval/cmake/ROS2.cmake

@@ -3,6 +3,8 @@ cmake_minimum_required(VERSION 3.3)
 # Find ROS build system
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 find_package(ov_core REQUIRED)
 
 # Describe ROS project
@@ -58,13 +60,10 @@ ament_export_libraries(ov_eval_lib)
 # Make binary files!
 ##################################################
 
-# TODO: UPGRADE THIS TO ROS2 AS ANOTHER FILE!!
-#if (catkin_FOUND AND ENABLE_ROS)
-#    add_executable(pose_to_file src/pose_to_file.cpp)
-#    target_link_libraries(pose_to_file ov_eval_lib ${thirdparty_libraries})
-#    add_executable(live_align_trajectory src/live_align_trajectory.cpp)
-#    target_link_libraries(live_align_trajectory ov_eval_lib ${thirdparty_libraries})
-#endif ()
+add_executable(pose_to_file src/pose_to_file_ros2.cpp)
+ament_target_dependencies(pose_to_file rclcpp geometry_msgs nav_msgs ov_core)
+target_link_libraries(pose_to_file ov_eval_lib ${thirdparty_libraries})
+install(TARGETS pose_to_file DESTINATION lib/${PROJECT_NAME})
 
 add_executable(format_converter src/format_converter.cpp)
 ament_target_dependencies(format_converter rclcpp ov_core)

ov_eval/package.xml

@@ -33,6 +33,8 @@
     <!-- ROS2: Dependencies needed to compile this package. -->
     <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
     <depend condition="$ROS_VERSION == 2">rclcpp</depend>
+    <depend condition="$ROS_VERSION == 2">geometry_msgs</depend>
+    <depend condition="$ROS_VERSION == 2">nav_msgs</depend>
     <depend condition="$ROS_VERSION == 2">ov_core</depend>
 
     <!-- System dependencies for both versions -->

ov_eval/src/pose_to_file_ros2.cpp

@@ -0,0 +1,84 @@
+/*
+ * 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 <https://www.gnu.org/licenses/>.
+ */
+
+#include "utils/Recorder.h"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <functional>
+#include <memory>
+#include <string>
+
+#include "utils/print.h"
+
+int main(int argc, char ** argv) {
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions options;
+  options.allow_undeclared_parameters(true);
+  options.automatically_declare_parameters_from_overrides(true);
+  auto node = std::make_shared<rclcpp::Node>("pose_to_file", options);
+
+  std::string verbosity;
+  node->get_parameter_or<std::string>("verbosity", verbosity, "INFO");
+  ov_core::Printer::setPrintLevel(verbosity);
+
+  std::string topic;
+  node->get_parameter_or<std::string>("topic", topic, "");
+  std::string topic_type;
+  node->get_parameter_or<std::string>("topic_type", topic_type, "");
+  std::string fileoutput;
+  node->get_parameter_or<std::string>("output", fileoutput, "");
+  if (topic.empty() || topic_type.empty() || fileoutput.empty()) {
+    PRINT_ERROR("Missing parameters: topic, topic_type, and output are required");
+    return EXIT_FAILURE;
+  }
+
+  PRINT_DEBUG("Done reading config values");
+  PRINT_DEBUG(" - topic = %s", topic.c_str());
+  PRINT_DEBUG(" - topic_type = %s", topic_type.c_str());
+  PRINT_DEBUG(" - file = %s", fileoutput.c_str());
+
+  ov_eval::Recorder recorder(fileoutput);
+  rclcpp::SubscriptionBase::SharedPtr sub;
+
+  if (topic_type == "PoseWithCovarianceStamped") {
+    sub = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+        topic, 9999, std::bind(&ov_eval::Recorder::callback_posecovariance, &recorder, std::placeholders::_1));
+  } else if (topic_type == "PoseStamped") {
+    sub = node->create_subscription<geometry_msgs::msg::PoseStamped>(
+        topic, 9999, std::bind(&ov_eval::Recorder::callback_pose, &recorder, std::placeholders::_1));
+  } else if (topic_type == "TransformStamped") {
+    sub = node->create_subscription<geometry_msgs::msg::TransformStamped>(
+        topic, 9999, std::bind(&ov_eval::Recorder::callback_transform, &recorder, std::placeholders::_1));
+  } else if (topic_type == "Odometry") {
+    sub = node->create_subscription<nav_msgs::msg::Odometry>(
+        topic, 9999, std::bind(&ov_eval::Recorder::callback_odometry, &recorder, std::placeholders::_1));
+  } else {
+    PRINT_ERROR("The specified topic type is not supported");
+    PRINT_ERROR("topic_type = %s", topic_type.c_str());
+    PRINT_ERROR("please select from: PoseWithCovarianceStamped, PoseStamped, TransformStamped, Odometry");
+    return EXIT_FAILURE;
+  }
+
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return EXIT_SUCCESS;
+}

ov_eval/src/utils/Recorder.h

@@ -29,11 +29,19 @@
 #include <Eigen/Eigen>
 #include <boost/filesystem.hpp>
 
+#if ROS_AVAILABLE == 1
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <nav_msgs/Odometry.h>
 #include <ros/ros.h>
+#elif ROS_AVAILABLE == 2
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include "utils/print.h"
+#endif
 
 namespace ov_eval {
 
@@ -56,19 +64,31 @@ class Recorder {
     // Create folder path to this location if not exists
     boost::filesystem::path dir(filename.c_str());
     if (boost::filesystem::create_directories(dir.parent_path())) {
+#if ROS_AVAILABLE == 1
       ROS_INFO("Created folder path to output file.");
       ROS_INFO("Path: %s", dir.parent_path().c_str());
+#elif ROS_AVAILABLE == 2
+      PRINT_DEBUG("Created folder path to output file: %s", dir.parent_path().c_str());
+#endif
     }
     // If it exists, then delete it
     if (boost::filesystem::exists(filename)) {
+#if ROS_AVAILABLE == 1
       ROS_WARN("Output file exists, deleting old file....");
+#elif ROS_AVAILABLE == 2
+      PRINT_DEBUG("Output file exists, deleting old file: %s", filename.c_str());
+#endif
       boost::filesystem::remove(filename);
     }
     // Open this file we want to write to
     outfile.open(filename.c_str());
     if (outfile.fail()) {
+#if ROS_AVAILABLE == 1
       ROS_ERROR("Unable to open output file!!");
       ROS_ERROR("Path: %s", filename.c_str());
+#elif ROS_AVAILABLE == 2
+      PRINT_ERROR("Unable to open output file: %s", filename.c_str());
+#endif
       std::exit(EXIT_FAILURE);
     }
     outfile << "# timestamp(s) tx ty tz qx qy qz qw Pr11 Pr12 Pr13 Pr22 Pr23 Pr33 Pt11 Pt12 Pt13 Pt22 Pt23 Pt33" << std::endl;
@@ -81,6 +101,29 @@ class Recorder {
     has_covariance = false;
   }
 
+  /**
+   * @brief Record a pose and write to file (ROS-agnostic; use from ROS2 or any source).
+   * @param ts timestamp in seconds
+   * @param p position (tx ty tz)
+   * @param q quaternion (qx qy qz qw)
+   * @param has_cov true if covariance is valid
+   * @param c_rot 3x3 rotation covariance (upper triangle written)
+   * @param c_pos 3x3 position covariance (upper triangle written)
+   */
+  void record_pose(double ts, const Eigen::Vector3d & p, const Eigen::Vector4d & q, bool has_cov,
+                   const Eigen::Matrix<double, 3, 3> & c_rot, const Eigen::Matrix<double, 3, 3> & c_pos) {
+    timestamp = ts;
+    p_IinG = p;
+    q_ItoG = q;
+    has_covariance = has_cov;
+    if (has_cov) {
+      cov_rot = c_rot;
+      cov_pos = c_pos;
+    }
+    write();
+  }
+
+#if ROS_AVAILABLE == 1
   /**
    * @brief Callback for nav_msgs::Odometry message types.
    *
@@ -146,6 +189,75 @@ class Recorder {
     p_IinG << msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z;
     write();
   }
+#elif ROS_AVAILABLE == 2
+  /**
+   * @brief Callback for nav_msgs::msg::Odometry message types.
+   *
+   * Note that covariance is in the order (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis).
+   * http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovariance.html
+   *
+   * @param msg New message
+   */
+  void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) {
+    timestamp = static_cast<double>(msg->header.stamp.sec) + 1e-9 * static_cast<double>(msg->header.stamp.nanosec);
+    q_ItoG << msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w;
+    p_IinG << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z;
+    cov_pos << msg->pose.covariance.at(0), msg->pose.covariance.at(1), msg->pose.covariance.at(2), msg->pose.covariance.at(6),
+        msg->pose.covariance.at(7), msg->pose.covariance.at(8), msg->pose.covariance.at(12), msg->pose.covariance.at(13),
+        msg->pose.covariance.at(14);
+    cov_rot << msg->pose.covariance.at(21), msg->pose.covariance.at(22), msg->pose.covariance.at(23), msg->pose.covariance.at(27),
+        msg->pose.covariance.at(28), msg->pose.covariance.at(29), msg->pose.covariance.at(33), msg->pose.covariance.at(34),
+        msg->pose.covariance.at(35);
+    has_covariance = true;
+    write();
+  }
+
+  /**
+   * @brief Callback for geometry_msgs::msg::PoseStamped message types
+   * @param msg New message
+   */
+  void callback_pose(const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
+    timestamp = static_cast<double>(msg->header.stamp.sec) + 1e-9 * static_cast<double>(msg->header.stamp.nanosec);
+    q_ItoG << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w;
+    p_IinG << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
+    has_covariance = false;
+    write();
+  }
+
+  /**
+   * @brief Callback for geometry_msgs::msg::PoseWithCovarianceStamped message types.
+   *
+   * Note that covariance is in the order (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis).
+   * http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovariance.html
+   *
+   * @param msg New message
+   */
+  void callback_posecovariance(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) {
+    timestamp = static_cast<double>(msg->header.stamp.sec) + 1e-9 * static_cast<double>(msg->header.stamp.nanosec);
+    q_ItoG << msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w;
+    p_IinG << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z;
+    cov_pos << msg->pose.covariance.at(0), msg->pose.covariance.at(1), msg->pose.covariance.at(2), msg->pose.covariance.at(6),
+        msg->pose.covariance.at(7), msg->pose.covariance.at(8), msg->pose.covariance.at(12), msg->pose.covariance.at(13),
+        msg->pose.covariance.at(14);
+    cov_rot << msg->pose.covariance.at(21), msg->pose.covariance.at(22), msg->pose.covariance.at(23), msg->pose.covariance.at(27),
+        msg->pose.covariance.at(28), msg->pose.covariance.at(29), msg->pose.covariance.at(33), msg->pose.covariance.at(34),
+        msg->pose.covariance.at(35);
+    has_covariance = true;
+    write();
+  }
+
+  /**
+   * @brief Callback for geometry_msgs::msg::TransformStamped message types
+   * @param msg New message
+   */
+  void callback_transform(const geometry_msgs::msg::TransformStamped::SharedPtr msg) {
+    timestamp = static_cast<double>(msg->header.stamp.sec) + 1e-9 * static_cast<double>(msg->header.stamp.nanosec);
+    q_ItoG << msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w;
+    p_IinG << msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z;
+    has_covariance = false;
+    write();
+  }
+#endif
 
 protected:
   /**

ov_msckf/cmake/ROS2.cmake

@@ -11,6 +11,8 @@ find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(image_transport REQUIRED)
+find_package(rosbag2_cpp REQUIRED)
+find_package(rosbag2_transport REQUIRED)
 find_package(ov_core REQUIRED)
 find_package(ov_init REQUIRED)
 
@@ -37,6 +39,8 @@ list(APPEND thirdparty_libraries
 )
 list(APPEND ament_libraries
         rclcpp
+        rosbag2_cpp
+        rosbag2_transport
         tf2_ros
         tf2_geometry_msgs
         std_msgs
@@ -93,6 +97,11 @@ ament_target_dependencies(run_subscribe_msckf ${ament_libraries})
 target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries})
 install(TARGETS run_subscribe_msckf DESTINATION lib/${PROJECT_NAME})
 
+add_executable(ros2_serial_msckf src/ros2_serial_msckf.cpp)
+ament_target_dependencies(ros2_serial_msckf ${ament_libraries})
+target_link_libraries(ros2_serial_msckf ov_msckf_lib ${thirdparty_libraries})
+install(TARGETS ros2_serial_msckf DESTINATION lib/${PROJECT_NAME})
+
 add_executable(run_simulation src/run_simulation.cpp)
 ament_target_dependencies(run_simulation ${ament_libraries})
 target_link_libraries(run_simulation ov_msckf_lib ${thirdparty_libraries})