From b63611b2c37e2ad452150bc09966018b621dae7d Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Tue, 18 Oct 2016 18:04:46 +0200 Subject: [PATCH] Updated to work with new APIs. (#125) --- .../cartographer_ros/node_main.cc | 39 ++++++++-------- .../cartographer_ros/sensor_bridge.cc | 44 +++++++++---------- 2 files changed, 42 insertions(+), 41 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index dcea646..1dacf41 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -33,7 +33,6 @@ #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/map_builder.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/local_trajectory_builder.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_options.h" #include "cartographer/mapping_3d/sparse_pose_graph.h" +#include "cartographer/sensor/collator.h" #include "cartographer/sensor/data.h" #include "cartographer/sensor/laser.h" #include "cartographer/sensor/point_cloud.h" @@ -121,7 +121,7 @@ class Node { void Initialize(); private: - void HandleSensorData(int64 timestamp, + void HandleSensorData(const string& sensor_id, std::unique_ptr sensor_data); bool HandleSubmapQuery( ::cartographer_ros_msgs::SubmapQuery::Request& request, @@ -266,9 +266,9 @@ void Node::Initialize() { CHECK_EQ(kTrajectoryBuilderId, map_builder_.AddTrajectoryBuilder()); sensor_collator_.AddTrajectory( kTrajectoryBuilderId, expected_sensor_identifiers, - [this](const int64 timestamp, + [this](const string& sensor_id, std::unique_ptr sensor_data) { - HandleSensorData(timestamp, std::move(sensor_data)); + HandleSensorData(sensor_id, std::move(sensor_data)); }); 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 sensor_data) { - auto it = rate_timers_.find(sensor_data->frame_id); + auto it = rate_timers_.find(sensor_id); if (it == rate_timers_.end()) { - it = rate_timers_ - .emplace(std::piecewise_construct, - std::forward_as_tuple(sensor_data->frame_id), - std::forward_as_tuple(carto::common::FromSeconds( - kSensorDataRatesLoggingPeriodSeconds))) - .first; + it = + rate_timers_ + .emplace(std::piecewise_construct, std::forward_as_tuple(sensor_id), + std::forward_as_tuple(carto::common::FromSeconds( + kSensorDataRatesLoggingPeriodSeconds))) + .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_ > 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(); } - const auto time = carto::common::FromUniversal(timestamp); auto* const trajectory_builder = map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId); switch (sensor_data->type) { 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); return; case carto::sensor::Data::Type::kLaserFan: 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 { 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; 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); return; } diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 376955e..76caf94 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -80,16 +80,15 @@ void SensorBridge::HandleOdometryMessage( translational, Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), rotational; // clang-format on - const auto sensor_to_tracking = - tf_bridge_->LookupToTracking(time, msg->child_frame_id); + const auto sensor_to_tracking = tf_bridge_->LookupToTracking( + time, CheckNoLeadingSlash(msg->child_frame_id)); if (sensor_to_tracking != nullptr) { sensor_collator_->AddSensorData( - trajectory_id_, carto::common::ToUniversal(time), topic, + trajectory_id_, topic, carto::common::make_unique<::cartographer::sensor::Data>( - CheckNoLeadingSlash(msg->child_frame_id), - ::cartographer::sensor::Data::Odometry{ - ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(), - covariance})); + time, ::cartographer::sensor::Data::Odometry{ + ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse(), + covariance})); } } @@ -98,17 +97,17 @@ void SensorBridge::HandleImuMessage(const string& topic, CHECK_NE(msg->linear_acceleration_covariance[0], -1); CHECK_NE(msg->angular_velocity_covariance[0], -1); const carto::common::Time time = FromRos(msg->header.stamp); - const auto sensor_to_tracking = - tf_bridge_->LookupToTracking(time, msg->header.frame_id); + const auto sensor_to_tracking = tf_bridge_->LookupToTracking( + time, CheckNoLeadingSlash(msg->header.frame_id)); if (sensor_to_tracking != nullptr) { CHECK(sensor_to_tracking->translation().norm() < 1e-5) << "The IMU frame must be colocated with the tracking frame. " "Transforming linear acceleration into the tracking frame will " "otherwise be imprecise."; sensor_collator_->AddSensorData( - trajectory_id_, carto::common::ToUniversal(time), topic, + trajectory_id_, topic, carto::common::make_unique<::cartographer::sensor::Data>( - CheckNoLeadingSlash(msg->header.frame_id), + time, ::cartographer::sensor::Data::Imu{ sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration), @@ -134,16 +133,15 @@ void SensorBridge::HandlePointCloud2Message( pcl::PointCloud pcl_point_cloud; pcl::fromROSMsg(*msg, pcl_point_cloud); const carto::common::Time time = FromRos(msg->header.stamp); - const auto sensor_to_tracking = - tf_bridge_->LookupToTracking(time, msg->header.frame_id); + const auto sensor_to_tracking = tf_bridge_->LookupToTracking( + time, CheckNoLeadingSlash(msg->header.frame_id)); if (sensor_to_tracking != nullptr) { sensor_collator_->AddSensorData( - trajectory_id_, carto::common::ToUniversal(time), topic, + trajectory_id_, topic, carto::common::make_unique<::cartographer::sensor::Data>( - CheckNoLeadingSlash(msg->header.frame_id), - carto::sensor::TransformLaserFan( - carto::sensor::FromProto(ToCartographer(pcl_point_cloud)), - sensor_to_tracking->cast()))); + time, carto::sensor::TransformLaserFan( + carto::sensor::FromProto(ToCartographer(pcl_point_cloud)), + sensor_to_tracking->cast()))); } } @@ -155,14 +153,14 @@ void SensorBridge::HandleLaserScanProto( laser_scan, options_.horizontal_laser_min_range, options_.horizontal_laser_max_range, 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) { sensor_collator_->AddSensorData( - trajectory_id_, carto::common::ToUniversal(time), topic, + trajectory_id_, topic, carto::common::make_unique<::cartographer::sensor::Data>( - CheckNoLeadingSlash(frame_id), - carto::sensor::TransformLaserFan( - laser_fan, sensor_to_tracking->cast()))); + time, carto::sensor::TransformLaserFan( + laser_fan, sensor_to_tracking->cast()))); } }