Use timing channel from PointCloud2, if available. (#896)
parent
5d5ce68a11
commit
82b5eb9688
|
@ -32,6 +32,9 @@
|
||||||
#include "geometry_msgs/Vector3.h"
|
#include "geometry_msgs/Vector3.h"
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
#include "nav_msgs/OccupancyGrid.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/ros.h"
|
||||||
#include "ros/serialization.h"
|
#include "ros/serialization.h"
|
||||||
#include "sensor_msgs/Imu.h"
|
#include "sensor_msgs/Imu.h"
|
||||||
|
@ -39,6 +42,34 @@
|
||||||
#include "sensor_msgs/MultiEchoLaserScan.h"
|
#include "sensor_msgs/MultiEchoLaserScan.h"
|
||||||
#include "sensor_msgs/PointCloud2.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 cartographer_ros {
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
|
@ -183,29 +214,63 @@ ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) {
|
||||||
|
|
||||||
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
|
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
|
||||||
::cartographer::common::Time>
|
::cartographer::common::Time>
|
||||||
ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message) {
|
ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
|
||||||
PointCloudWithIntensities point_cloud;
|
PointCloudWithIntensities point_cloud;
|
||||||
// We check for intensity field here to avoid run-time warnings if we pass in
|
// We check for intensity field here to avoid run-time warnings if we pass in
|
||||||
// a PointCloud2 without intensity.
|
// 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::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) {
|
for (const auto& point : pcl_point_cloud) {
|
||||||
point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f);
|
point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f);
|
||||||
point_cloud.intensities.push_back(point.intensity);
|
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 {
|
} else {
|
||||||
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
||||||
pcl::fromROSMsg(message, pcl_point_cloud);
|
pcl::fromROSMsg(msg, pcl_point_cloud);
|
||||||
|
point_cloud.points.reserve(pcl_point_cloud.size());
|
||||||
// If we don't have an intensity field, just copy XYZ and fill in
|
point_cloud.intensities.reserve(pcl_point_cloud.size());
|
||||||
// 1.0.
|
|
||||||
for (const auto& point : 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.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) {
|
LandmarkData ToLandmarkData(const LandmarkList& landmark_list) {
|
||||||
|
|
|
@ -17,7 +17,6 @@
|
||||||
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
|
#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
|
||||||
#define 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/common/time.h"
|
||||||
#include "cartographer/io/submap_painter.h"
|
#include "cartographer/io/submap_painter.h"
|
||||||
#include "cartographer/sensor/landmark_data.h"
|
#include "cartographer/sensor/landmark_data.h"
|
||||||
|
@ -28,9 +27,6 @@
|
||||||
#include "geometry_msgs/Transform.h"
|
#include "geometry_msgs/Transform.h"
|
||||||
#include "geometry_msgs/TransformStamped.h"
|
#include "geometry_msgs/TransformStamped.h"
|
||||||
#include "nav_msgs/OccupancyGrid.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/Imu.h"
|
||||||
#include "sensor_msgs/LaserScan.h"
|
#include "sensor_msgs/LaserScan.h"
|
||||||
#include "sensor_msgs/MultiEchoLaserScan.h"
|
#include "sensor_msgs/MultiEchoLaserScan.h"
|
||||||
|
@ -63,7 +59,7 @@ ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg);
|
||||||
|
|
||||||
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
|
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
|
||||||
::cartographer::common::Time>
|
::cartographer::common::Time>
|
||||||
ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message);
|
ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg);
|
||||||
|
|
||||||
::cartographer::sensor::LandmarkData ToLandmarkData(
|
::cartographer::sensor::LandmarkData ToLandmarkData(
|
||||||
const cartographer_ros_msgs::LandmarkList& landmark_list);
|
const cartographer_ros_msgs::LandmarkList& landmark_list);
|
||||||
|
|
|
@ -165,14 +165,10 @@ void SensorBridge::HandleMultiEchoLaserScanMessage(
|
||||||
void SensorBridge::HandlePointCloud2Message(
|
void SensorBridge::HandlePointCloud2Message(
|
||||||
const std::string& sensor_id,
|
const std::string& sensor_id,
|
||||||
const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||||
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
carto::sensor::PointCloudWithIntensities point_cloud;
|
||||||
pcl::fromROSMsg(*msg, pcl_point_cloud);
|
carto::common::Time time;
|
||||||
carto::sensor::TimedPointCloud point_cloud;
|
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
|
||||||
for (const auto& point : pcl_point_cloud) {
|
HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
|
const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
|
||||||
|
@ -222,6 +218,9 @@ void SensorBridge::HandleLaserScan(
|
||||||
void SensorBridge::HandleRangefinder(
|
void SensorBridge::HandleRangefinder(
|
||||||
const std::string& sensor_id, const carto::common::Time time,
|
const std::string& sensor_id, const carto::common::Time time,
|
||||||
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
|
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
|
||||||
|
if (!ranges.empty()) {
|
||||||
|
CHECK_LE(ranges.back()[3], 0);
|
||||||
|
}
|
||||||
const auto sensor_to_tracking =
|
const auto sensor_to_tracking =
|
||||||
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
|
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
|
||||||
if (sensor_to_tracking != nullptr) {
|
if (sensor_to_tracking != nullptr) {
|
||||||
|
|
Loading…
Reference in New Issue