From 07020e8c0311e85ff7af89aac6c0078cf5e30885 Mon Sep 17 00:00:00 2001
From: Benedikt Mersch <benedikt.mersch@gmail.com>
Date: Wed, 18 Dec 2024 15:05:11 +0100
Subject: [PATCH] Publish map with correct frame

---
 ros/src/OdometryServer.cpp | 5 ++++-
 ros/src/Utils.hpp          | 9 ---------
 2 files changed, 4 insertions(+), 10 deletions(-)

diff --git a/ros/src/OdometryServer.cpp b/ros/src/OdometryServer.cpp
index 687c3b58..0e752931 100644
--- a/ros/src/OdometryServer.cpp
+++ b/ros/src/OdometryServer.cpp
@@ -200,7 +200,10 @@ void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame,
 
     frame_publisher_->publish(std::move(EigenToPointCloud2(frame, header)));
     kpoints_publisher_->publish(std::move(EigenToPointCloud2(keypoints, header)));
-    map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, kiss_pose, header)));
+
+    auto local_map_header = header;
+    local_map_header.frame_id = lidar_odom_frame_;
+    map_publisher_->publish(std::move(EigenToPointCloud2(kiss_map, local_map_header)));
 }
 }  // namespace kiss_icp_ros
 
diff --git a/ros/src/Utils.hpp b/ros/src/Utils.hpp
index 09c9cd8e..ade93d8f 100644
--- a/ros/src/Utils.hpp
+++ b/ros/src/Utils.hpp
@@ -228,13 +228,4 @@ inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::
                    [&](const auto &point) { return T * point; });
     return EigenToPointCloud2(points_t, header);
 }
-
-inline std::unique_ptr<PointCloud2> EigenToPointCloud2(const std::vector<Eigen::Vector3d> &points,
-                                                       const std::vector<double> &timestamps,
-                                                       const Header &header) {
-    auto msg = CreatePointCloud2Msg(points.size(), header, true);
-    FillPointCloud2XYZ(points, *msg);
-    FillPointCloud2Timestamp(timestamps, *msg);
-    return msg;
-}
 }  // namespace kiss_icp_ros::utils