Use timing channel from PointCloud2, if available. (#896)

master
Juraj Oršulić 2018-06-12 15:33:34 +02:00 committed by Wally B. Feed
parent 5d5ce68a11
commit 82b5eb9688
3 changed files with 89 additions and 29 deletions

View File

@ -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")) {
if (PointCloud2HasField(msg, "intensity")) {
if (PointCloud2HasField(msg, "time")) {
pcl::PointCloud<PointXYZIT> 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::PointXYZI> pcl_point_cloud;
pcl::fromROSMsg(message, 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 {
// If we don't have an intensity field, just copy XYZ and fill in 1.0f.
if (PointCloud2HasField(msg, "time")) {
pcl::PointCloud<PointXYZT> 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::PointXYZ> 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.
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.0);
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) {

View File

@ -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);

View File

@ -165,14 +165,10 @@ void SensorBridge::HandleMultiEchoLaserScanMessage(
void SensorBridge::HandlePointCloud2Message(
const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg) {
pcl::PointCloud<pcl::PointXYZ> 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) {