Revise timestamping (#18)

* Query from last stamp in case of frame drops * Rename * WIP * Fix conversions * Fix build * Make CI happy * Add TimeStampHandler module to handle this madness * Minor renaming * Try to take care of absolute timestamping of points + scan stamped at the end * Comments * Should fix all cases that came to my mind at this time * Some comments to make stuff more clear * Remove useless case, we need to fix deskewing * Fix condition for clarity --------- Co-authored-by: tizianoGuadagnino <[email protected]>
PRBonn · Nov 21, 2024 · b441180 · b441180
1 parent 49f0030
commit b441180
Show file tree

Hide file tree

Showing 8 changed files with 209 additions and 114 deletions.
diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt
@@ -58,8 +58,8 @@ set(dependencies
 add_library(
   kinematic_icp_ros SHARED
   src/kinematic_icp_ros/server/LidarOdometryServer.cpp src/kinematic_icp_ros/utils/RosUtils.cpp
-  src/kinematic_icp_ros/utils/RosbagUtils.cpp src/kinematic_icp_ros/nodes/online_node.cpp
-)# Adding it here for composition
+  src/kinematic_icp_ros/utils/RosbagUtils.cpp src/kinematic_icp_ros/utils/TimeStampHandler.cpp
+  src/kinematic_icp_ros/nodes/online_node.cpp) # Adding it here for composition
 target_compile_features(kinematic_icp_ros PUBLIC cxx_std_17)
 target_include_directories(kinematic_icp_ros PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
 target_link_libraries(kinematic_icp_ros kinematic_icp_pipeline "${cpp_typesupport_target}")

diff --git a/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp b/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp
@@ -23,14 +23,14 @@
 #pragma once
 
 #include "kinematic_icp/pipeline/KinematicICP.hpp"
+#include "kinematic_icp_ros/utils/TimeStampHandler.hpp"
 
 // ROS 2 C
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 
 // ROS 2 C++
-#include <builtin_interfaces/msg/time.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/time.hpp>
@@ -57,7 +57,7 @@ class LidarOdometryServer {
     /// Register new frame
     void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
     std::unique_ptr<kinematic_icp::pipeline::KinematicICP> kinematic_icp_;
-    builtin_interfaces::msg::Time current_stamp_;
+    utils::TimeStampHandler timestamps_handler_;
 
 private:
     /// Stream the estimated pose to ROS

diff --git a/ros/include/kinematic_icp_ros/utils/RosUtils.hpp b/ros/include/kinematic_icp_ros/utils/RosUtils.hpp
@@ -129,15 +129,6 @@ inline Sophus::SE3d LookupDeltaTransform(const std::string &target_frame,
     }
 }
 
-std::optional<PointField> GetTimestampField(const PointCloud2::ConstSharedPtr msg);
-
-std::vector<double> NormalizeTimestamps(const std::vector<double> &timestamps);
-
-auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg,
-                              const PointField &timestamp_field);
-
-std::vector<double> GetTimestamps(const PointCloud2::ConstSharedPtr msg);
-
 std::vector<Eigen::Vector3d> PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg,
                                                 const Sophus::SE3d &T = {});
 

diff --git a/ros/include/kinematic_icp_ros/utils/TimeStampHandler.hpp b/ros/include/kinematic_icp_ros/utils/TimeStampHandler.hpp
@@ -0,0 +1,52 @@
+// MIT License
+
+// Copyright (c) 2024 Tiziano Guadagnino, Benedikt Mersch, Ignacio Vizzo, Cyrill
+// Stachniss.
+
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+#pragma once
+#include <tf2/time.h>
+
+#include <builtin_interfaces/msg/time.hpp>
+#include <rclcpp/duration.hpp>
+#include <rclcpp/time.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tuple>
+#include <vector>
+
+namespace kinematic_icp_ros::utils {
+
+using StampType = builtin_interfaces::msg::Time;
+
+struct TimeStampHandler {
+    std::tuple<StampType, StampType, std::vector<double>> ProcessTimestamps(
+        const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg);
+    // From double to ROS TimeStamp
+    inline StampType toStamp(const double &time_in_seconds) const {
+        return rclcpp::Time(tf2::durationFromSec(time_in_seconds).count());
+    };
+    // From ROS TimeStamp to double
+    inline double toTime(const StampType &stamp) const {
+        return rclcpp::Time(stamp).nanoseconds() * 1e-9;
+    };
+
+    StampType last_processed_stamp_;
+};
+
+}  // namespace kinematic_icp_ros::utils
diff --git a/ros/src/kinematic_icp_ros/nodes/offline_node.cpp b/ros/src/kinematic_icp_ros/nodes/offline_node.cpp
@@ -113,7 +113,7 @@ void OfflineNode::Run() {
     while (rclcpp::ok() && bag_multiplexer_.IsMessageAvailable()) {
         const auto msg = GetNextMsg();
         odometry_server_->RegisterFrame(msg);
-        const auto &stamp = odometry_server_->current_stamp_;
+        const auto &stamp = odometry_server_->timestamps_handler_.last_processed_stamp_;
         poses_with_timestamps_.emplace_back(stamp.sec + stamp.nanosec * 1e-9,
                                             odometry_server_->kinematic_icp_->pose());
         bar.tick();

diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp
@@ -179,7 +179,7 @@ void LidarOdometryServer::InitializePoseAndExtrinsic(
 
     // Initialization finished
     RCLCPP_INFO(node_->get_logger(), "KISS-ICP ROS 2 odometry node initialized");
-    current_stamp_ = msg->header.stamp;
+    timestamps_handler_.last_processed_stamp_ = msg->header.stamp;
     initialize_odom_node = true;
 }
 
@@ -189,39 +189,27 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con
     }
 
     // Buffer the last state This will be used for computing the veloicty
-    const auto last_stamp = current_stamp_;
     const auto last_pose = kinematic_icp_->pose();
-    // Extract timestamps
-    const auto timestamps = GetTimestamps(msg);
-    const auto &[min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend());
-    // From double to ROS TimeStamp
-    auto toStamp = [](const double &time) -> builtin_interfaces::msg::Time {
-        return rclcpp::Time(tf2::durationFromSec(time).count());
-    };
-    // Update what is the current stamp of this iteration
-    const auto begin_scan_stamp = min_it != timestamps.cend() ? toStamp(*min_it) : last_stamp;
-    const auto end_scan_stamp = max_it != timestamps.cend() ? toStamp(*max_it) : msg->header.stamp;
-    current_stamp_ = end_scan_stamp;
+    const auto &[begin_odom_query, end_odom_query, timestamps] =
+        timestamps_handler_.ProcessTimestamps(msg);
+
     // Get the initial guess from the wheel odometry
     const auto delta =
-        LookupDeltaTransform(base_frame_, begin_scan_stamp, base_frame_, end_scan_stamp,
+        LookupDeltaTransform(base_frame_, begin_odom_query, base_frame_, end_odom_query,
                              wheel_odom_frame_, tf_timeout_, tf2_buffer_);
 
     // Run kinematic ICP
     if (delta.log().norm() > 1e-3) {
         const auto &extrinsic = sensor_to_base_footprint_;
         const auto points = PointCloud2ToEigen(msg, {});
-        const auto normalized_timestamps = NormalizeTimestamps(timestamps);
         const auto &[frame, kpoints] =
-            kinematic_icp_->RegisterFrame(points, normalized_timestamps, extrinsic, delta);
+            kinematic_icp_->RegisterFrame(points, timestamps, extrinsic, delta);
         PublishClouds(frame, kpoints);
     }
 
-    auto toTime = [](const builtin_interfaces::msg::Time &stamp) -> double {
-        return rclcpp::Time(stamp).nanoseconds() * 1e-9;
-    };
     // Compute velocities, use the elapsed time between the current msg and the last received
-    const double elapsed_time = toTime(current_stamp_) - toTime(last_stamp);
+    const double elapsed_time =
+        timestamps_handler_.toTime(end_odom_query) - timestamps_handler_.toTime(begin_odom_query);
     const Sophus::SE3d::Tangent delta_twist = (last_pose.inverse() * kinematic_icp_->pose()).log();
     const Sophus::SE3d::Tangent velocity = delta_twist / elapsed_time;
 
@@ -237,15 +225,15 @@ void LidarOdometryServer::PublishOdometryMsg(const Sophus::SE3d &pose,
             if (invert_odom_tf_) return tf2::sophusToTransform(pose.inverse());
             return tf2::sophusToTransform(pose);
         }();
-        tf_msg_.header.stamp = current_stamp_;
+        tf_msg_.header.stamp = timestamps_handler_.last_processed_stamp_;
         tf_broadcaster_->sendTransform(tf_msg_);
     }
 
     // publish odometry msg
     odom_msg_.pose.pose = tf2::sophusToPose(pose);
     odom_msg_.twist.twist.linear.x = velocity[0];
     odom_msg_.twist.twist.angular.z = velocity[5];
-    odom_msg_.header.stamp = current_stamp_;
+    odom_msg_.header.stamp = timestamps_handler_.last_processed_stamp_;
     odom_publisher_->publish(odom_msg_);
 }
 
@@ -254,12 +242,12 @@ void LidarOdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame
     // For re-publishing the input frame and keypoints, we do it in the LiDAR coordinate frames
     std_msgs::msg::Header lidar_header;
     lidar_header.frame_id = base_frame_;
-    lidar_header.stamp = current_stamp_;
+    lidar_header.stamp = timestamps_handler_.last_processed_stamp_;
 
     // The internal map representation is in the lidar_odom_frame_
     std_msgs::msg::Header map_header;
     map_header.frame_id = lidar_odom_frame_;
-    map_header.stamp = current_stamp_;
+    map_header.stamp = timestamps_handler_.last_processed_stamp_;
 
     // Check for subscriptions before publishing to avoid unnecesary CPU usage
     if (frame_publisher_->get_subscription_count() > 0) {

diff --git a/ros/src/kinematic_icp_ros/utils/RosUtils.cpp b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp
@@ -27,82 +27,6 @@
 
 namespace kinematic_icp_ros::utils {
 
-std::optional<PointField> GetTimestampField(const PointCloud2::ConstSharedPtr msg) {
-    PointField timestamp_field;
-    for (const auto &field : msg->fields) {
-        if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) {
-            timestamp_field = field;
-        }
-    }
-    if (timestamp_field.count) return timestamp_field;
-    RCLCPP_WARN_ONCE(rclcpp::get_logger("kinematic_icp_ros"),
-                     "Field 't', 'timestamp', or 'time'  does not exist. "
-                     "Disabling scan deskewing");
-    return {};
-}
-
-// Normalize timestamps from 0.0 to 1.0
-std::vector<double> NormalizeTimestamps(const std::vector<double> &timestamps) {
-    const auto [min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend());
-    const double min_timestamp = min_it != timestamps.cend() ? *min_it : 0.0;
-    const double max_timestamp = max_it != timestamps.cend() ? *max_it : 1.0;
-
-    std::vector<double> timestamps_normalized(timestamps.size());
-    std::transform(timestamps.cbegin(), timestamps.cend(), timestamps_normalized.begin(),
-                   [&](const auto &timestamp) {
-                       return (timestamp - min_timestamp) / (max_timestamp - min_timestamp);
-                   });
-    return timestamps_normalized;
-}
-
-auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg,
-                              const PointField &timestamp_field) {
-    auto number_of_digits_decimal_part = [](const auto &stamp) {
-        const uint64_t number_of_seconds = static_cast<uint64_t>(std::round(stamp));
-        return number_of_seconds > 0 ? std::floor(std::log10(number_of_seconds) + 1) : 1;
-    };
-    auto extract_timestamps =
-        [&]<typename T>(sensor_msgs::PointCloud2ConstIterator<T> &&it) -> std::vector<double> {
-        const size_t n_points = msg->height * msg->width;
-        std::vector<double> timestamps;
-        timestamps.reserve(n_points);
-        for (size_t i = 0; i < n_points; ++i, ++it) {
-            double stampd = static_cast<double>(*it);
-            // If the number of digits is greater than 10 (which is the maximum number of digits
-            // that can be represented with a 32 bits integer), the stamp is in nanoseconds instead
-            // of seconds, perform conversion
-            if (number_of_digits_decimal_part(stampd) > 10) {
-                stampd *= 1e-9;
-            }
-            timestamps.emplace_back(stampd);
-        }
-        return timestamps;
-    };
-
-    // According to the type of the timestamp == type, return a PointCloud2ConstIterator<type>
-    using sensor_msgs::PointCloud2ConstIterator;
-    if (timestamp_field.datatype == PointField::UINT32) {
-        return extract_timestamps(PointCloud2ConstIterator<uint32_t>(*msg, timestamp_field.name));
-    } else if (timestamp_field.datatype == PointField::FLOAT32) {
-        return extract_timestamps(PointCloud2ConstIterator<float>(*msg, timestamp_field.name));
-    } else if (timestamp_field.datatype == PointField::FLOAT64) {
-        return extract_timestamps(PointCloud2ConstIterator<double>(*msg, timestamp_field.name));
-    }
-
-    // timestamp type not supported, please open an issue :)
-    throw std::runtime_error("timestamp field type not supported");
-}
-
-std::vector<double> GetTimestamps(const PointCloud2::ConstSharedPtr msg) {
-    auto timestamp_field = GetTimestampField(msg);
-    if (!timestamp_field.has_value()) return {};
-
-    // Extract timestamps from cloud_msg
-    std::vector<double> timestamps = ExtractTimestampsFromMsg(msg, timestamp_field.value());
-
-    return timestamps;
-}
-
 std::vector<Eigen::Vector3d> PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg,
                                                 const Sophus::SE3d &T) {
     std::vector<Eigen::Vector3d> points;