diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 3cbf145..b4c356f 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -87,13 +87,15 @@ std::unique_ptr HandleMessage( points_batch->start_time = start_time; points_batch->frame_id = message.header.frame_id; - carto::sensor::PointCloudWithIntensities point_cloud = + ::cartographer::sensor::PointCloudWithIntensities point_cloud; + ::cartographer::common::Time point_cloud_time; + std::tie(point_cloud, point_cloud_time) = ToPointCloudWithIntensities(message); CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size()); for (size_t i = 0; i < point_cloud.points.size(); ++i) { const carto::common::Time time = - start_time + carto::common::FromSeconds(point_cloud.points[i][3]); + point_cloud_time + carto::common::FromSeconds(point_cloud.points[i][3]); if (!transform_interpolation_buffer.Has(time)) { continue; } diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 5da7098..9707b60 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -91,8 +91,8 @@ float GetFirstEcho(const sensor_msgs::LaserEcho& echo) { // For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan. template -PointCloudWithIntensities LaserScanToPointCloudWithIntensities( - const LaserMessageType& msg) { +std::tuple +LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) { CHECK_GE(msg.range_min, 0.f); CHECK_GE(msg.range_max, msg.range_min); if (msg.angle_increment > 0.f) { @@ -124,7 +124,15 @@ PointCloudWithIntensities LaserScanToPointCloudWithIntensities( } angle += msg.angle_increment; } - return point_cloud; + ::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 (auto& point : point_cloud.points) { + point[3] -= duration; + } + } + return std::make_tuple(point_cloud, timestamp); } bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2, @@ -153,18 +161,21 @@ sensor_msgs::PointCloud2 ToPointCloud2Message( return msg; } -PointCloudWithIntensities ToPointCloudWithIntensities( - const sensor_msgs::LaserScan& msg) { +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) { return LaserScanToPointCloudWithIntensities(msg); } -PointCloudWithIntensities ToPointCloudWithIntensities( - const sensor_msgs::MultiEchoLaserScan& msg) { +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) { return LaserScanToPointCloudWithIntensities(msg); } -PointCloudWithIntensities ToPointCloudWithIntensities( - const sensor_msgs::PointCloud2& message) { +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message) { PointCloudWithIntensities point_cloud; // We check for intensity field here to avoid run-time warnings if we pass in // a PointCloud2 without intensity. @@ -186,7 +197,7 @@ PointCloudWithIntensities ToPointCloudWithIntensities( point_cloud.intensities.push_back(1.0); } } - return point_cloud; + return std::make_tuple(point_cloud, FromRos(message.header.stamp)); } Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) { diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index ee2f576..d6a7ed6 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -18,6 +18,7 @@ #define CARTOGRAPHER_ROS_MSG_CONVERSION_H_ #include "cartographer/common/port.h" +#include "cartographer/common/time.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "geometry_msgs/Pose.h" @@ -45,14 +46,20 @@ geometry_msgs::Pose ToGeometryMsgPose( geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d); -::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( - const sensor_msgs::LaserScan& msg); +// Converts ROS message to point cloud. Returns the time when the last point +// was acquired (different from the ROS timestamp). Timing of points is given in +// the fourth component of each point relative to `Time`. +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg); -::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( - const sensor_msgs::MultiEchoLaserScan& msg); +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg); -::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities( - const sensor_msgs::PointCloud2& message); +std::tuple<::cartographer::sensor::PointCloudWithIntensities, + ::cartographer::common::Time> +ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message); ::cartographer::transform::Rigid3d ToRigid3d( const geometry_msgs::TransformStamped& transform); diff --git a/cartographer_ros/cartographer_ros/msg_conversion_test.cc b/cartographer_ros/cartographer_ros/msg_conversion_test.cc index 5239b07..33720c7 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion_test.cc @@ -35,7 +35,8 @@ TEST(MsgConversion, LaserScanToPointCloud) { laser_scan.range_min = 0.f; laser_scan.range_max = 10.f; - const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points; + const auto point_cloud = + std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; EXPECT_TRUE( point_cloud[0].isApprox(Eigen::Vector4f(1.f, 0.f, 0.f, 0.f), 1e-6)); EXPECT_TRUE(point_cloud[1].isApprox( @@ -71,7 +72,8 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) { laser_scan.range_min = 2.f; laser_scan.range_max = 10.f; - const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points; + const auto point_cloud = + std::get<0>(ToPointCloudWithIntensities(laser_scan)).points; ASSERT_EQ(2, point_cloud.size()); EXPECT_TRUE( point_cloud[0].isApprox(Eigen::Vector4f(0.f, 2.f, 0.f, 0.f), 1e-6)); diff --git a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc index 1a17593..572dcb6 100644 --- a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc +++ b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc @@ -174,7 +174,7 @@ class RangeDataChecker { template RangeChecksum ComputeRangeChecksum(const MessageType& message) { const cartographer::sensor::TimedPointCloud& point_cloud = - ToPointCloudWithIntensities(message).points; + std::get<0>(ToPointCloudWithIntensities(message)).points; Eigen::Vector4f points_sum = Eigen::Vector4f::Zero(); for (const auto& point : point_cloud) { points_sum += point; diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 5d750a4..7b5499b 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -116,15 +116,19 @@ void SensorBridge::HandleImuMessage(const std::string& sensor_id, void SensorBridge::HandleLaserScanMessage( const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { - HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, - ToPointCloudWithIntensities(*msg)); + ::cartographer::sensor::PointCloudWithIntensities point_cloud; + ::cartographer::common::Time time; + std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); + HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud); } void SensorBridge::HandleMultiEchoLaserScanMessage( const std::string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { - HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, - ToPointCloudWithIntensities(*msg)); + ::cartographer::sensor::PointCloudWithIntensities point_cloud; + ::cartographer::common::Time time; + std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); + HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud); } void SensorBridge::HandlePointCloud2Message( @@ -143,24 +147,30 @@ void SensorBridge::HandlePointCloud2Message( const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; } void SensorBridge::HandleLaserScan( - const std::string& sensor_id, const carto::common::Time start_time, + const std::string& sensor_id, const carto::common::Time time, const std::string& frame_id, const carto::sensor::PointCloudWithIntensities& points) { + CHECK_LE(points.points.back()[3], 0); // TODO(gaschler): Use per-point time instead of subdivisions. for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { const size_t start_index = points.points.size() * i / num_subdivisions_per_laser_scan_; const size_t end_index = points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_; - const carto::sensor::TimedPointCloud subdivision( + carto::sensor::TimedPointCloud subdivision( points.points.begin() + start_index, points.points.begin() + end_index); if (start_index == end_index) { continue; } - const size_t middle_index = (start_index + end_index) / 2; + const double time_to_subdivision_end = subdivision.back()[3]; + // `subdivision_time` is the end of the measurement so sensor::Collator will + // send all other sensor data first. const carto::common::Time subdivision_time = - start_time + - carto::common::FromSeconds(points.points.at(middle_index)[3]); + time + carto::common::FromSeconds(time_to_subdivision_end); + for (auto& point : subdivision) { + point[3] -= time_to_subdivision_end; + } + CHECK_EQ(subdivision.back()[3], 0); HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision); } }