From b44118053cf2666012acddb461381ed463de63c8 Mon Sep 17 00:00:00 2001 From: Benedikt Mersch Date: Tue, 19 Nov 2024 11:18:36 +0100 Subject: [PATCH] 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 --- ros/CMakeLists.txt | 4 +- .../server/LidarOdometryServer.hpp | 4 +- .../kinematic_icp_ros/utils/RosUtils.hpp | 9 -- .../utils/TimeStampHandler.hpp | 52 +++++++ .../kinematic_icp_ros/nodes/offline_node.cpp | 2 +- .../server/LidarOdometryServer.cpp | 36 ++--- ros/src/kinematic_icp_ros/utils/RosUtils.cpp | 76 ---------- .../utils/TimeStampHandler.cpp | 140 ++++++++++++++++++ 8 files changed, 209 insertions(+), 114 deletions(-) create mode 100644 ros/include/kinematic_icp_ros/utils/TimeStampHandler.hpp create mode 100644 ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index d4e2678..932dc5c 100644 --- 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 index 5bbdbf9..2b709f1 100644 --- a/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp +++ b/ros/include/kinematic_icp_ros/server/LidarOdometryServer.hpp @@ -23,6 +23,7 @@ #pragma once #include "kinematic_icp/pipeline/KinematicICP.hpp" +#include "kinematic_icp_ros/utils/TimeStampHandler.hpp" // ROS 2 C #include @@ -30,7 +31,6 @@ #include // ROS 2 C++ -#include #include #include #include @@ -57,7 +57,7 @@ class LidarOdometryServer { /// Register new frame void RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg); std::unique_ptr 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 index 5001fba..5610d00 100644 --- 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 GetTimestampField(const PointCloud2::ConstSharedPtr msg); - -std::vector NormalizeTimestamps(const std::vector ×tamps); - -auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, - const PointField ×tamp_field); - -std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg); - std::vector 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 new file mode 100644 index 0000000..7ec6a1e --- /dev/null +++ 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 + +#include +#include +#include +#include +#include +#include + +namespace kinematic_icp_ros::utils { + +using StampType = builtin_interfaces::msg::Time; + +struct TimeStampHandler { + std::tuple> 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 index 898ac00..a7c9577 100644 --- 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 index ab39990..fadfc4a 100644 --- 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,7 +225,7 @@ 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_); } @@ -245,7 +233,7 @@ void LidarOdometryServer::PublishOdometryMsg(const Sophus::SE3d &pose, 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 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 index 3f840e6..a952e8f 100644 --- 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 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 NormalizeTimestamps(const std::vector ×tamps) { - 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 timestamps_normalized(timestamps.size()); - std::transform(timestamps.cbegin(), timestamps.cend(), timestamps_normalized.begin(), - [&](const auto ×tamp) { - return (timestamp - min_timestamp) / (max_timestamp - min_timestamp); - }); - return timestamps_normalized; -} - -auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, - const PointField ×tamp_field) { - auto number_of_digits_decimal_part = [](const auto &stamp) { - const uint64_t number_of_seconds = static_cast(std::round(stamp)); - return number_of_seconds > 0 ? std::floor(std::log10(number_of_seconds) + 1) : 1; - }; - auto extract_timestamps = - [&](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { - const size_t n_points = msg->height * msg->width; - std::vector timestamps; - timestamps.reserve(n_points); - for (size_t i = 0; i < n_points; ++i, ++it) { - double stampd = static_cast(*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 - using sensor_msgs::PointCloud2ConstIterator; - if (timestamp_field.datatype == PointField::UINT32) { - return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); - } else if (timestamp_field.datatype == PointField::FLOAT32) { - return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); - } else if (timestamp_field.datatype == PointField::FLOAT64) { - return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); - } - - // timestamp type not supported, please open an issue :) - throw std::runtime_error("timestamp field type not supported"); -} - -std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg) { - auto timestamp_field = GetTimestampField(msg); - if (!timestamp_field.has_value()) return {}; - - // Extract timestamps from cloud_msg - std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field.value()); - - return timestamps; -} - std::vector PointCloud2ToEigen(const PointCloud2::ConstSharedPtr msg, const Sophus::SE3d &T) { std::vector points; diff --git a/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp b/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp new file mode 100644 index 0000000..d8ce8bb --- /dev/null +++ b/ros/src/kinematic_icp_ros/utils/TimeStampHandler.cpp @@ -0,0 +1,140 @@ +// 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. +#include "kinematic_icp_ros/utils/TimeStampHandler.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +using PointCloud2 = sensor_msgs::msg::PointCloud2; +using PointField = sensor_msgs::msg::PointField; +using Header = std_msgs::msg::Header; + +std::optional 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 {}; +} + +auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, + const PointField ×tamp_field) { + auto number_of_digits_decimal_part = [](const auto &stamp) { + const uint64_t number_of_seconds = static_cast(std::round(stamp)); + return number_of_seconds > 0 ? std::floor(std::log10(number_of_seconds) + 1) : 1; + }; + auto extract_timestamps = + [&](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { + const size_t n_points = msg->height * msg->width; + std::vector timestamps; + timestamps.reserve(n_points); + for (size_t i = 0; i < n_points; ++i, ++it) { + double stampd = static_cast(*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 + using sensor_msgs::PointCloud2ConstIterator; + if (timestamp_field.datatype == PointField::UINT32) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } else if (timestamp_field.datatype == PointField::FLOAT32) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } else if (timestamp_field.datatype == PointField::FLOAT64) { + return extract_timestamps(PointCloud2ConstIterator(*msg, timestamp_field.name)); + } + + // timestamp type not supported, please open an issue :) + throw std::runtime_error("timestamp field type not supported"); +} + +std::vector GetTimestamps(const PointCloud2::ConstSharedPtr msg) { + auto timestamp_field = GetTimestampField(msg); + if (!timestamp_field.has_value()) return {}; + + // Extract timestamps from cloud_msg + std::vector timestamps = ExtractTimestampsFromMsg(msg, timestamp_field.value()); + + return timestamps; +} +} // namespace + +namespace kinematic_icp_ros::utils { + +std::tuple> TimeStampHandler::ProcessTimestamps( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { + std::vector timestamps = GetTimestamps(msg); + const auto &[min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); + const StampType msg_stamp = msg->header.stamp; + const StampType begin_stamp = last_processed_stamp_; + StampType end_stamp = msg_stamp; + if (max_it != timestamps.cend()) { + const double &max_stamp_in_seconds = *max_it; + const double &min_stamp_in_seconds = *min_it; + const double msg_stamp_in_seconds = this->toTime(msg_stamp); + + // Check if stamping happens and the beginning or the end of scan + const bool is_stamped_at_the_beginning = + std::abs(msg_stamp_in_seconds - max_stamp_in_seconds) > 1e-8; + if (is_stamped_at_the_beginning) { + // begin-stamping -> add scan duration to the stamp + const auto scan_duration = + tf2::durationFromSec(max_stamp_in_seconds - min_stamp_in_seconds); + end_stamp = StampType(rclcpp::Time(end_stamp) + scan_duration); + } + + // Normalize timestamps + std::transform(timestamps.cbegin(), timestamps.cend(), timestamps.begin(), + [&](const auto ×tamp) { + return (timestamp - min_stamp_in_seconds) / + (max_stamp_in_seconds - min_stamp_in_seconds); + }); + } + last_processed_stamp_ = end_stamp; + return std::make_tuple(begin_stamp, end_stamp, timestamps); +} + +} // namespace kinematic_icp_ros::utils