From aac00343dba54a8fa395edbf71380fe399d35b5c Mon Sep 17 00:00:00 2001 From: Ken Date: Tue, 22 Oct 2024 22:10:48 +0200 Subject: [PATCH 1/2] Prevent negative sec and nano in TimeToROSStamp when time is large, replaced Time functions with standard methods. --- .../server/LidarOdometryServer.cpp | 21 ++++--------------- 1 file changed, 4 insertions(+), 17 deletions(-) diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp index 9ee60e4..98d192c 100644 --- a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp +++ b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp @@ -34,7 +34,6 @@ #include "kinematic_icp_ros/utils/RosUtils.hpp" // ROS 2 headers -#include #include #include @@ -54,19 +53,7 @@ namespace { using milliseconds = std::chrono::milliseconds; using seconds = std::chrono::duration; using std::chrono::duration_cast; - -// Convert to ros time -auto TimeToROSStamp(const double &time) { - builtin_interfaces::msg::Time stamp; - stamp.sec = static_cast(std::floor(time)); - stamp.nanosec = static_cast((time - stamp.sec) * 1e9); - return stamp; -}; -double ROSStampToTime(const builtin_interfaces::msg::Time stamp) { - double time = static_cast(stamp.sec); - time += static_cast(stamp.nanosec) * 1e-9; - return time; -}; +using Time = builtin_interfaces::msg::Time; } // namespace namespace kinematic_icp_ros { @@ -199,9 +186,9 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con const auto &[min_it, max_it] = std::minmax_element(timestamps.cbegin(), timestamps.cend()); // Update what is the current stamp of this iteration const auto begin_scan_stamp = - min_it != timestamps.cend() ? TimeToROSStamp(*min_it) : last_stamp; + min_it != timestamps.cend() ? Time{rclcpp::Time(*min_it)} : last_stamp; const auto end_scan_stamp = - max_it != timestamps.cend() ? TimeToROSStamp(*max_it) : msg->header.stamp; + max_it != timestamps.cend() ? Time{rclcpp::Time(*max_it)} : msg->header.stamp; current_stamp_ = end_scan_stamp; // Get the initial guess from the wheel odometry const auto delta = @@ -219,7 +206,7 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con } // Compute velocities, use the elapsed time between the current msg and the last received - const double elapsed_time = ROSStampToTime(end_scan_stamp) - ROSStampToTime(begin_scan_stamp); + const double elapsed_time = rclcpp::Time(current_stamp_).nanoseconds() - rclcpp::Time(last_stamp).nanoseconds(); const Sophus::SE3d::Tangent delta_twist = (last_pose.inverse() * kinematic_icp_->pose()).log(); const Sophus::SE3d::Tangent velocity = delta_twist / elapsed_time; From febfe6c979d94bb80b15737e01cff24026ddffa3 Mon Sep 17 00:00:00 2001 From: tizianoGuadagnino Date: Wed, 23 Oct 2024 17:06:26 +0200 Subject: [PATCH 2/2] Wrap standard ROS functions into small lambdas for readability --- .../server/LidarOdometryServer.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp index 98d192c..b1c9bb9 100644 --- a/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp +++ b/ros/src/kinematic_icp_ros/server/LidarOdometryServer.cpp @@ -34,6 +34,7 @@ #include "kinematic_icp_ros/utils/RosUtils.hpp" // ROS 2 headers +#include #include #include @@ -53,7 +54,6 @@ namespace { using milliseconds = std::chrono::milliseconds; using seconds = std::chrono::duration; using std::chrono::duration_cast; -using Time = builtin_interfaces::msg::Time; } // namespace namespace kinematic_icp_ros { @@ -184,11 +184,13 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con // 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() ? Time{rclcpp::Time(*min_it)} : last_stamp; - const auto end_scan_stamp = - max_it != timestamps.cend() ? Time{rclcpp::Time(*max_it)} : msg->header.stamp; + 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; // Get the initial guess from the wheel odometry const auto delta = @@ -205,8 +207,11 @@ void LidarOdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::Con 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 = rclcpp::Time(current_stamp_).nanoseconds() - rclcpp::Time(last_stamp).nanoseconds(); + const double elapsed_time = toTime(current_stamp_) - toTime(last_stamp); const Sophus::SE3d::Tangent delta_twist = (last_pose.inverse() * kinematic_icp_->pose()).log(); const Sophus::SE3d::Tangent velocity = delta_twist / elapsed_time;