diff --git a/ros/src/kinematic_icp_ros/utils/RosUtils.cpp b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp index 356328a..3f840e6 100644 --- a/ros/src/kinematic_icp_ros/utils/RosUtils.cpp +++ b/ros/src/kinematic_icp_ros/utils/RosUtils.cpp @@ -30,8 +30,7 @@ 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" || - field.name == "stamps")) { + if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) { timestamp_field = field; } } @@ -62,7 +61,6 @@ auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, 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; }; - const double msg_stamp_in_sec = rclcpp::Time(msg->header.stamp).nanoseconds() * 1e-9; auto extract_timestamps = [&](sensor_msgs::PointCloud2ConstIterator &&it) -> std::vector { const size_t n_points = msg->height * msg->width; @@ -76,9 +74,6 @@ auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg, if (number_of_digits_decimal_part(stampd) > 10) { stampd *= 1e-9; } - if (stampd < msg_stamp_in_sec) { - stampd += msg_stamp_in_sec; - } timestamps.emplace_back(stampd); } return timestamps;