From 18d08bd2265a94cbbfcf04328416a6b5cd4ab570 Mon Sep 17 00:00:00 2001
From: tizianoGuadagnino <frevo93@gmail.com>
Date: Fri, 18 Oct 2024 09:48:57 +0200
Subject: [PATCH] Output filename was at times broken if an absolute path was
 provided, fix it

---
 .../kinematic_icp_ros/nodes/offline_node.hpp     |  4 ++--
 ros/src/kinematic_icp_ros/nodes/offline_node.cpp | 16 ++++++++--------
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/ros/include/kinematic_icp_ros/nodes/offline_node.hpp b/ros/include/kinematic_icp_ros/nodes/offline_node.hpp
index 186a1f8..1abcfd8 100644
--- a/ros/include/kinematic_icp_ros/nodes/offline_node.hpp
+++ b/ros/include/kinematic_icp_ros/nodes/offline_node.hpp
@@ -26,6 +26,7 @@
 #include <string>
 
 // ROS
+#include <filesystem>
 #include <rclcpp/node.hpp>
 #include <rclcpp/node_options.hpp>
 #include <sophus/se3.hpp>
@@ -51,8 +52,7 @@ class OfflineNode {
     // store data for the experiments
     using PoseWithStamp = std::pair<double, Sophus::SE3d>;
     std::vector<PoseWithStamp> poses_with_timestamps_;
-    std::string output_dir_;
-    std::string poses_filename_;
+    std::filesystem::path output_pose_file_;
 
     // Offline node specifics
     BagMultiplexer bag_multiplexer_;
diff --git a/ros/src/kinematic_icp_ros/nodes/offline_node.cpp b/ros/src/kinematic_icp_ros/nodes/offline_node.cpp
index a268bc0..898ac00 100644
--- a/ros/src/kinematic_icp_ros/nodes/offline_node.cpp
+++ b/ros/src/kinematic_icp_ros/nodes/offline_node.cpp
@@ -38,12 +38,12 @@
 #include "kinematic_icp_ros/utils/indicators.hpp"
 
 namespace {
-std::string generateOutputFilename(const std::string &bag_filename) {
+std::filesystem::path generateOutputFilename(const std::string &bag_filename) {
     size_t last_dot = bag_filename.find_last_of(".");
     std::string output_file = bag_filename.substr(0, last_dot);
     output_file += "_kinematic_icp_poses_tum.txt";
     std::filesystem::path output_path(output_file);
-    return output_path.filename().string();
+    return output_path.filename();
 }
 }  // namespace
 
@@ -55,21 +55,21 @@ OfflineNode::OfflineNode(const rclcpp::NodeOptions &options) {
     odometry_server_ = std::make_shared<LidarOdometryServer>(node_);
 
     auto bag_filename = node_->declare_parameter<std::string>("bag_filename");
-    poses_filename_ = generateOutputFilename(bag_filename);
-    output_dir_ = node_->declare_parameter<std::string>("output_dir");
+    const auto poses_filename = generateOutputFilename(bag_filename);
+    output_pose_file_ = std::filesystem::path(node_->declare_parameter<std::string>("output_dir"));
+    output_pose_file_ /= poses_filename;
     auto tf_bridge = std::make_shared<BufferableBag::TFBridge>(node_);
     bag_multiplexer_.AddBag(BufferableBag(bag_filename, tf_bridge, pcl_topic_));
 }
 
 void OfflineNode::writePosesInTumFormat() {
-    const std::string output_file = output_dir_ + poses_filename_;
-    std::ofstream file(output_file);
+    std::ofstream file(output_pose_file_);
     if (!file.is_open()) {
-        std::cerr << "Error opening file: " << output_file << std::endl;
+        std::cerr << "Error opening file: " << output_pose_file_ << std::endl;
         return;
     }
 
-    RCLCPP_INFO_STREAM(node_->get_logger(), "Saving poses in TUM format in " << output_file);
+    RCLCPP_INFO_STREAM(node_->get_logger(), "Saving poses in TUM format in " << output_pose_file_);
 
     // Iterate over the poses and timestamps
     for (const auto &[timestamp, pose] : poses_with_timestamps_) {