Updated to work with new APIs. (#125)
parent
caa77c8588
commit
b63611b2c3
|
@ -33,7 +33,6 @@
|
||||||
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
#include "cartographer/mapping/global_trajectory_builder_interface.h"
|
||||||
#include "cartographer/mapping/map_builder.h"
|
#include "cartographer/mapping/map_builder.h"
|
||||||
#include "cartographer/mapping/proto/submaps.pb.h"
|
#include "cartographer/mapping/proto/submaps.pb.h"
|
||||||
#include "cartographer/sensor/collator.h"
|
|
||||||
#include "cartographer/mapping_2d/global_trajectory_builder.h"
|
#include "cartographer/mapping_2d/global_trajectory_builder.h"
|
||||||
#include "cartographer/mapping_2d/local_trajectory_builder.h"
|
#include "cartographer/mapping_2d/local_trajectory_builder.h"
|
||||||
#include "cartographer/mapping_2d/sparse_pose_graph.h"
|
#include "cartographer/mapping_2d/sparse_pose_graph.h"
|
||||||
|
@ -41,6 +40,7 @@
|
||||||
#include "cartographer/mapping_3d/local_trajectory_builder.h"
|
#include "cartographer/mapping_3d/local_trajectory_builder.h"
|
||||||
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
|
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
|
||||||
#include "cartographer/mapping_3d/sparse_pose_graph.h"
|
#include "cartographer/mapping_3d/sparse_pose_graph.h"
|
||||||
|
#include "cartographer/sensor/collator.h"
|
||||||
#include "cartographer/sensor/data.h"
|
#include "cartographer/sensor/data.h"
|
||||||
#include "cartographer/sensor/laser.h"
|
#include "cartographer/sensor/laser.h"
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
|
@ -121,7 +121,7 @@ class Node {
|
||||||
void Initialize();
|
void Initialize();
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void HandleSensorData(int64 timestamp,
|
void HandleSensorData(const string& sensor_id,
|
||||||
std::unique_ptr<carto::sensor::Data> sensor_data);
|
std::unique_ptr<carto::sensor::Data> sensor_data);
|
||||||
bool HandleSubmapQuery(
|
bool HandleSubmapQuery(
|
||||||
::cartographer_ros_msgs::SubmapQuery::Request& request,
|
::cartographer_ros_msgs::SubmapQuery::Request& request,
|
||||||
|
@ -266,9 +266,9 @@ void Node::Initialize() {
|
||||||
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
|
CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder());
|
||||||
sensor_collator_.AddTrajectory(
|
sensor_collator_.AddTrajectory(
|
||||||
kTrajectoryBuilderId, expected_sensor_identifiers,
|
kTrajectoryBuilderId, expected_sensor_identifiers,
|
||||||
[this](const int64 timestamp,
|
[this](const string& sensor_id,
|
||||||
std::unique_ptr<carto::sensor::Data> sensor_data) {
|
std::unique_ptr<carto::sensor::Data> sensor_data) {
|
||||||
HandleSensorData(timestamp, std::move(sensor_data));
|
HandleSensorData(sensor_id, std::move(sensor_data));
|
||||||
});
|
});
|
||||||
|
|
||||||
submap_list_publisher_ =
|
submap_list_publisher_ =
|
||||||
|
@ -490,18 +490,18 @@ void Node::SpinOccupancyGridThreadForever() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::HandleSensorData(const int64 timestamp,
|
void Node::HandleSensorData(const string& sensor_id,
|
||||||
std::unique_ptr<carto::sensor::Data> sensor_data) {
|
std::unique_ptr<carto::sensor::Data> sensor_data) {
|
||||||
auto it = rate_timers_.find(sensor_data->frame_id);
|
auto it = rate_timers_.find(sensor_id);
|
||||||
if (it == rate_timers_.end()) {
|
if (it == rate_timers_.end()) {
|
||||||
it = rate_timers_
|
it =
|
||||||
.emplace(std::piecewise_construct,
|
rate_timers_
|
||||||
std::forward_as_tuple(sensor_data->frame_id),
|
.emplace(std::piecewise_construct, std::forward_as_tuple(sensor_id),
|
||||||
std::forward_as_tuple(carto::common::FromSeconds(
|
std::forward_as_tuple(carto::common::FromSeconds(
|
||||||
kSensorDataRatesLoggingPeriodSeconds)))
|
kSensorDataRatesLoggingPeriodSeconds)))
|
||||||
.first;
|
.first;
|
||||||
}
|
}
|
||||||
it->second.Pulse(carto::common::FromUniversal(timestamp));
|
it->second.Pulse(sensor_data->time);
|
||||||
|
|
||||||
if (std::chrono::steady_clock::now() - last_sensor_data_rates_logging_time_ >
|
if (std::chrono::steady_clock::now() - last_sensor_data_rates_logging_time_ >
|
||||||
carto::common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
|
carto::common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) {
|
||||||
|
@ -511,26 +511,29 @@ void Node::HandleSensorData(const int64 timestamp,
|
||||||
last_sensor_data_rates_logging_time_ = std::chrono::steady_clock::now();
|
last_sensor_data_rates_logging_time_ = std::chrono::steady_clock::now();
|
||||||
}
|
}
|
||||||
|
|
||||||
const auto time = carto::common::FromUniversal(timestamp);
|
|
||||||
auto* const trajectory_builder =
|
auto* const trajectory_builder =
|
||||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
|
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId);
|
||||||
switch (sensor_data->type) {
|
switch (sensor_data->type) {
|
||||||
case carto::sensor::Data::Type::kImu:
|
case carto::sensor::Data::Type::kImu:
|
||||||
trajectory_builder->AddImuData(time, sensor_data->imu.linear_acceleration,
|
trajectory_builder->AddImuData(sensor_data->time,
|
||||||
|
sensor_data->imu.linear_acceleration,
|
||||||
sensor_data->imu.angular_velocity);
|
sensor_data->imu.angular_velocity);
|
||||||
return;
|
return;
|
||||||
|
|
||||||
case carto::sensor::Data::Type::kLaserFan:
|
case carto::sensor::Data::Type::kLaserFan:
|
||||||
if (options_.map_builder_options.use_trajectory_builder_2d()) {
|
if (options_.map_builder_options.use_trajectory_builder_2d()) {
|
||||||
trajectory_builder->AddHorizontalLaserFan(time, sensor_data->laser_fan);
|
trajectory_builder->AddHorizontalLaserFan(sensor_data->time,
|
||||||
|
sensor_data->laser_fan);
|
||||||
} else {
|
} else {
|
||||||
CHECK(options_.map_builder_options.use_trajectory_builder_3d());
|
CHECK(options_.map_builder_options.use_trajectory_builder_3d());
|
||||||
trajectory_builder->AddLaserFan3D(time, sensor_data->laser_fan);
|
trajectory_builder->AddLaserFan3D(sensor_data->time,
|
||||||
|
sensor_data->laser_fan);
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
|
|
||||||
case carto::sensor::Data::Type::kOdometry:
|
case carto::sensor::Data::Type::kOdometry:
|
||||||
trajectory_builder->AddOdometerPose(time, sensor_data->odometry.pose,
|
trajectory_builder->AddOdometerPose(sensor_data->time,
|
||||||
|
sensor_data->odometry.pose,
|
||||||
sensor_data->odometry.covariance);
|
sensor_data->odometry.covariance);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
|
@ -80,16 +80,15 @@ void SensorBridge::HandleOdometryMessage(
|
||||||
translational, Eigen::Matrix3d::Zero(),
|
translational, Eigen::Matrix3d::Zero(),
|
||||||
Eigen::Matrix3d::Zero(), rotational;
|
Eigen::Matrix3d::Zero(), rotational;
|
||||||
// clang-format on
|
// clang-format on
|
||||||
const auto sensor_to_tracking =
|
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
|
||||||
tf_bridge_->LookupToTracking(time, msg->child_frame_id);
|
time, CheckNoLeadingSlash(msg->child_frame_id));
|
||||||
if (sensor_to_tracking != nullptr) {
|
if (sensor_to_tracking != nullptr) {
|
||||||
sensor_collator_->AddSensorData(
|
sensor_collator_->AddSensorData(
|
||||||
trajectory_id_, carto::common::ToUniversal(time), topic,
|
trajectory_id_, topic,
|
||||||
carto::common::make_unique<::cartographer::sensor::Data>(
|
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||||
CheckNoLeadingSlash(msg->child_frame_id),
|
time, ::cartographer::sensor::Data::Odometry{
|
||||||
::cartographer::sensor::Data::Odometry{
|
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
|
||||||
ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(),
|
covariance}));
|
||||||
covariance}));
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -98,17 +97,17 @@ void SensorBridge::HandleImuMessage(const string& topic,
|
||||||
CHECK_NE(msg->linear_acceleration_covariance[0], -1);
|
CHECK_NE(msg->linear_acceleration_covariance[0], -1);
|
||||||
CHECK_NE(msg->angular_velocity_covariance[0], -1);
|
CHECK_NE(msg->angular_velocity_covariance[0], -1);
|
||||||
const carto::common::Time time = FromRos(msg->header.stamp);
|
const carto::common::Time time = FromRos(msg->header.stamp);
|
||||||
const auto sensor_to_tracking =
|
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
|
||||||
tf_bridge_->LookupToTracking(time, msg->header.frame_id);
|
time, CheckNoLeadingSlash(msg->header.frame_id));
|
||||||
if (sensor_to_tracking != nullptr) {
|
if (sensor_to_tracking != nullptr) {
|
||||||
CHECK(sensor_to_tracking->translation().norm() < 1e-5)
|
CHECK(sensor_to_tracking->translation().norm() < 1e-5)
|
||||||
<< "The IMU frame must be colocated with the tracking frame. "
|
<< "The IMU frame must be colocated with the tracking frame. "
|
||||||
"Transforming linear acceleration into the tracking frame will "
|
"Transforming linear acceleration into the tracking frame will "
|
||||||
"otherwise be imprecise.";
|
"otherwise be imprecise.";
|
||||||
sensor_collator_->AddSensorData(
|
sensor_collator_->AddSensorData(
|
||||||
trajectory_id_, carto::common::ToUniversal(time), topic,
|
trajectory_id_, topic,
|
||||||
carto::common::make_unique<::cartographer::sensor::Data>(
|
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||||
CheckNoLeadingSlash(msg->header.frame_id),
|
time,
|
||||||
::cartographer::sensor::Data::Imu{
|
::cartographer::sensor::Data::Imu{
|
||||||
sensor_to_tracking->rotation() *
|
sensor_to_tracking->rotation() *
|
||||||
ToEigen(msg->linear_acceleration),
|
ToEigen(msg->linear_acceleration),
|
||||||
|
@ -134,16 +133,15 @@ void SensorBridge::HandlePointCloud2Message(
|
||||||
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
|
||||||
pcl::fromROSMsg(*msg, pcl_point_cloud);
|
pcl::fromROSMsg(*msg, pcl_point_cloud);
|
||||||
const carto::common::Time time = FromRos(msg->header.stamp);
|
const carto::common::Time time = FromRos(msg->header.stamp);
|
||||||
const auto sensor_to_tracking =
|
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(
|
||||||
tf_bridge_->LookupToTracking(time, msg->header.frame_id);
|
time, CheckNoLeadingSlash(msg->header.frame_id));
|
||||||
if (sensor_to_tracking != nullptr) {
|
if (sensor_to_tracking != nullptr) {
|
||||||
sensor_collator_->AddSensorData(
|
sensor_collator_->AddSensorData(
|
||||||
trajectory_id_, carto::common::ToUniversal(time), topic,
|
trajectory_id_, topic,
|
||||||
carto::common::make_unique<::cartographer::sensor::Data>(
|
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||||
CheckNoLeadingSlash(msg->header.frame_id),
|
time, carto::sensor::TransformLaserFan(
|
||||||
carto::sensor::TransformLaserFan(
|
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
|
||||||
carto::sensor::FromProto(ToCartographer(pcl_point_cloud)),
|
sensor_to_tracking->cast<float>())));
|
||||||
sensor_to_tracking->cast<float>())));
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -155,14 +153,14 @@ void SensorBridge::HandleLaserScanProto(
|
||||||
laser_scan, options_.horizontal_laser_min_range,
|
laser_scan, options_.horizontal_laser_min_range,
|
||||||
options_.horizontal_laser_max_range,
|
options_.horizontal_laser_max_range,
|
||||||
options_.horizontal_laser_missing_echo_ray_length);
|
options_.horizontal_laser_missing_echo_ray_length);
|
||||||
const auto sensor_to_tracking = tf_bridge_->LookupToTracking(time, frame_id);
|
const auto sensor_to_tracking =
|
||||||
|
tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id));
|
||||||
if (sensor_to_tracking != nullptr) {
|
if (sensor_to_tracking != nullptr) {
|
||||||
sensor_collator_->AddSensorData(
|
sensor_collator_->AddSensorData(
|
||||||
trajectory_id_, carto::common::ToUniversal(time), topic,
|
trajectory_id_, topic,
|
||||||
carto::common::make_unique<::cartographer::sensor::Data>(
|
carto::common::make_unique<::cartographer::sensor::Data>(
|
||||||
CheckNoLeadingSlash(frame_id),
|
time, carto::sensor::TransformLaserFan(
|
||||||
carto::sensor::TransformLaserFan(
|
laser_fan, sensor_to_tracking->cast<float>())));
|
||||||
laser_fan, sensor_to_tracking->cast<float>())));
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue