From 82b5eb96884b48f4e77672fdd8d169bac9882d26 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Tue, 12 Jun 2018 15:33:34 +0200 Subject: [PATCH] Use timing channel from PointCloud2, if available. (#896) --- .../cartographer_ros/msg_conversion.cc | 97 ++++++++++++++++--- .../cartographer_ros/msg_conversion.h | 6 +- .../cartographer_ros/sensor_bridge.cc | 15 ++- 3 files changed, 89 insertions(+), 29 deletions(-) diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 2548005..bc73b58 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -32,6 +32,9 @@ #include "geometry_msgs/Vector3.h" #include "glog/logging.h" #include "nav_msgs/OccupancyGrid.h" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "pcl_conversions/pcl_conversions.h" #include "ros/ros.h" #include "ros/serialization.h" #include "sensor_msgs/Imu.h" @@ -39,6 +42,34 @@ #include "sensor_msgs/MultiEchoLaserScan.h" #include "sensor_msgs/PointCloud2.h" +namespace { + +// Sizes of PCL point types have to be 4n floats for alignment, as described in +// http://pointclouds.org/documentation/tutorials/adding_custom_ptype.php +struct PointXYZT { + float x; + float y; + float z; + float time; +}; + +struct PointXYZIT { + PCL_ADD_POINT4D; + float intensity; + float time; + float unused_padding[2]; +}; + +} // namespace + +POINT_CLOUD_REGISTER_POINT_STRUCT( + PointXYZT, (float, x, x)(float, y, y)(float, z, z)(float, time, time)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + PointXYZIT, + (float, x, x)(float, y, y)(float, z, z)(float, intensity, + intensity)(float, time, time)) + namespace cartographer_ros { namespace { @@ -183,29 +214,63 @@ ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) { std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message) { +ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) { PointCloudWithIntensities point_cloud; // We check for intensity field here to avoid run-time warnings if we pass in // a PointCloud2 without intensity. - if (PointCloud2HasField(message, "intensity")) { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(message, pcl_point_cloud); - for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f); - point_cloud.intensities.push_back(point.intensity); + if (PointCloud2HasField(msg, "intensity")) { + if (PointCloud2HasField(msg, "time")) { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.emplace_back(point.x, point.y, point.z, point.time); + point_cloud.intensities.push_back(point.intensity); + } + } else { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f); + point_cloud.intensities.push_back(point.intensity); + } } } else { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(message, pcl_point_cloud); - - // If we don't have an intensity field, just copy XYZ and fill in - // 1.0. - for (const auto& point : pcl_point_cloud) { - point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f); - point_cloud.intensities.push_back(1.0); + // If we don't have an intensity field, just copy XYZ and fill in 1.0f. + if (PointCloud2HasField(msg, "time")) { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.emplace_back(point.x, point.y, point.z, point.time); + point_cloud.intensities.push_back(1.0f); + } + } else { + pcl::PointCloud pcl_point_cloud; + pcl::fromROSMsg(msg, pcl_point_cloud); + point_cloud.points.reserve(pcl_point_cloud.size()); + point_cloud.intensities.reserve(pcl_point_cloud.size()); + for (const auto& point : pcl_point_cloud) { + point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f); + point_cloud.intensities.push_back(1.0f); + } } } - return std::make_tuple(point_cloud, FromRos(message.header.stamp)); + ::cartographer::common::Time timestamp = FromRos(msg.header.stamp); + if (!point_cloud.points.empty()) { + const double duration = point_cloud.points.back()[3]; + timestamp += cartographer::common::FromSeconds(duration); + for (Eigen::Vector4f& point : point_cloud.points) { + point[3] -= duration; + CHECK_LE(point[3], 0) << "Encountered a point with a larger stamp than " + "the last point in the cloud."; + } + } + return std::make_tuple(point_cloud, timestamp); } LandmarkData ToLandmarkData(const LandmarkList& landmark_list) { diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index b0ab3a8..ab3fa1e 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -17,7 +17,6 @@ #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H -#include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/io/submap_painter.h" #include "cartographer/sensor/landmark_data.h" @@ -28,9 +27,6 @@ #include "geometry_msgs/Transform.h" #include "geometry_msgs/TransformStamped.h" #include "nav_msgs/OccupancyGrid.h" -#include "pcl/point_cloud.h" -#include "pcl/point_types.h" -#include "pcl_conversions/pcl_conversions.h" #include "sensor_msgs/Imu.h" #include "sensor_msgs/LaserScan.h" #include "sensor_msgs/MultiEchoLaserScan.h" @@ -63,7 +59,7 @@ ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg); std::tuple<::cartographer::sensor::PointCloudWithIntensities, ::cartographer::common::Time> -ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message); +ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg); ::cartographer::sensor::LandmarkData ToLandmarkData( const cartographer_ros_msgs::LandmarkList& landmark_list); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 416efe4..d85dcf5 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -165,14 +165,10 @@ void SensorBridge::HandleMultiEchoLaserScanMessage( void SensorBridge::HandlePointCloud2Message( const std::string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg) { - pcl::PointCloud pcl_point_cloud; - pcl::fromROSMsg(*msg, pcl_point_cloud); - carto::sensor::TimedPointCloud point_cloud; - for (const auto& point : pcl_point_cloud) { - point_cloud.emplace_back(point.x, point.y, point.z, 0.f); - } - HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, - point_cloud); + carto::sensor::PointCloudWithIntensities point_cloud; + carto::common::Time time; + std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); + HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points); } const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; } @@ -222,6 +218,9 @@ void SensorBridge::HandleLaserScan( void SensorBridge::HandleRangefinder( const std::string& sensor_id, const carto::common::Time time, const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) { + if (!ranges.empty()) { + CHECK_LE(ranges.back()[3], 0); + } const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); if (sensor_to_tracking != nullptr) {