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 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