diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 0ac719745b..f9e4caf509 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -3,7 +3,9 @@ # Version 2.13.5: UNRELEASED - Changes in libraries: - \ref mrpt_ros2bridge_grp: - - mrpt::ros2bridge::toROS() conversion from `PointCloud2` to mrpt::maps::CPointsMapXYZIRT: recognize timestamp field names `"t"` and `"timestamp"`, apart of `"time"`. + - mrpt::ros2bridge::toROS() conversion from `PointCloud2` to mrpt::maps::CPointsMapXYZIRT: + - Recognize timestamp field names `"t"` and `"timestamp"`, apart of `"time"`. + - Fixed detection of timestamp fields in `Float64` format, and convert them from absolute to relative timestamps. - Build system: - This main MRPT repository is no longer directly built as a ROS package. Please, use the wrappers for better modularity: - https://github.com/MRPT/mrpt_ros diff --git a/libs/maps/src/obs/CObservationPointCloud.cpp b/libs/maps/src/obs/CObservationPointCloud.cpp index 9cb31a8a78..ea0dcd02fd 100644 --- a/libs/maps/src/obs/CObservationPointCloud.cpp +++ b/libs/maps/src/obs/CObservationPointCloud.cpp @@ -70,8 +70,8 @@ void CObservationPointCloud::getDescriptionAsText(std::ostream& o) const { float Tmin, Tmax; mrpt::math::minimum_maximum(*ptrTs, Tmin, Tmax); - o << "Timestamp channel values: min=" << Tmin << " max=" << Tmax << " (" << ptrTs->size() - << " entries)\n"; + o << mrpt::format("Timestamp channel values: min=%f max=%f", Tmin, Tmax); + o << "(" << ptrTs->size() << " entries)\n"; } if (auto* ptrRs = pointcloud->getPointsBufferRef_ring(); ptrRs && !ptrRs->empty()) { diff --git a/libs/ros2bridge/src/point_cloud2.cpp b/libs/ros2bridge/src/point_cloud2.cpp index 631719e803..b5806c8538 100644 --- a/libs/ros2bridge/src/point_cloud2.cpp +++ b/libs/ros2bridge/src/point_cloud2.cpp @@ -53,12 +53,27 @@ static void get_float_from_field( { if (field->datatype == sensor_msgs::msg::PointField::FLOAT32) output = *(reinterpret_cast(&data[field->offset])); - else - output = (float)(*(reinterpret_cast(&data[field->offset]))); + else if (field->datatype == sensor_msgs::msg::PointField::FLOAT64) + output = static_cast(*(reinterpret_cast(&data[field->offset]))); } else output = 0.0; } + +static void get_double_from_field( + const sensor_msgs::msg::PointField* field, const unsigned char* data, double& output) +{ + if (field != nullptr) + { + if (field->datatype == sensor_msgs::msg::PointField::FLOAT32) + output = static_cast(*(reinterpret_cast(&data[field->offset]))); + else if (field->datatype == sensor_msgs::msg::PointField::FLOAT64) + output = *(reinterpret_cast(&data[field->offset])); + } + else + output = 0.0; +} + static void get_uint16_from_field( const sensor_msgs::msg::PointField* field, const unsigned char* data, uint16_t& output) { @@ -190,6 +205,7 @@ bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud2& msg, CPoints obj.resize_XYZIRT(num_points, !!i_field, !!r_field, !!t_field); unsigned int idx = 0; + std::optional baseTimeStamp; for (unsigned int row = 0; row < msg.height; ++row) { const unsigned char* row_data = &msg.data[row * msg.row_step]; @@ -217,9 +233,22 @@ bool mrpt::ros2bridge::fromROS(const sensor_msgs::msg::PointCloud2& msg, CPoints } if (t_field) { - float t; - get_float_from_field(t_field, msg_data, t); - obj.setPointTime(idx, t); + double t; + get_double_from_field(t_field, msg_data, t); + + // If the sensor uses absolute timestamp, convert them to relative + // since otherwise precision is lost in the double->float conversion: + if (std::abs(t) > 5.0) + { + // It looks like absolute timestamps, convert to relative: + if (!baseTimeStamp) baseTimeStamp = t; + obj.setPointTime(idx, static_cast(t - *baseTimeStamp)); + } + else + { + // It looks relative timestamps: + obj.setPointTime(idx, static_cast(t)); + } } } }