From 534f0119c461aaf8fd7b04a1a37f016672372996 Mon Sep 17 00:00:00 2001 From: Damon Kohler Date: Tue, 20 Sep 2016 08:42:56 +0200 Subject: [PATCH] Transforms odometry into the tracking frame. (#65) --- .../src/cartographer_node_main.cc | 28 ++++++++++++------- cartographer_ros/src/sensor_data.cc | 7 ++--- cartographer_ros/src/sensor_data.h | 14 +++++----- cartographer_ros/src/sensor_data_producer.cc | 5 ++-- 4 files changed, 30 insertions(+), 24 deletions(-) diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 533ab79..b932572 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -204,8 +204,8 @@ class Node { void HandleSensorData(int64 timestamp, std::unique_ptr sensor_data); - void AddOdometry(int64 timestamp, const string& frame_id, const Rigid3d& pose, - const PoseCovariance& covariance); + void AddOdometry(int64 timestamp, const string& frame_id, + const SensorData::Odometry& odometry); void AddImu(int64 timestamp, const string& frame_id, const proto::Imu& imu); void AddHorizontalLaserFan(int64 timestamp, const string& frame_id, const proto::LaserScan& laser_scan); @@ -286,10 +286,10 @@ Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time, ::ros::Duration(options_.lookup_transform_timeout_sec))); } -void Node::AddOdometry(int64 timestamp, const string& frame_id, - const Rigid3d& pose, const PoseCovariance& covariance) { +void Node::AddOdometry(const int64 timestamp, const string& frame_id, + const SensorData::Odometry& odometry) { const carto::common::Time time = carto::common::FromUniversal(timestamp); - PoseCovariance applied_covariance = covariance; + PoseCovariance applied_covariance = odometry.covariance; if (options_.use_constant_odometry_variance) { const Eigen::Matrix3d translational = Eigen::Matrix3d::Identity() * @@ -303,8 +303,16 @@ void Node::AddOdometry(int64 timestamp, const string& frame_id, Eigen::Matrix3d::Zero(), rotational; // clang-format on } - map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) - ->AddOdometerPose(time, pose, applied_covariance); + try { + const Rigid3d sensor_to_tracking = + LookupToTrackingTransformOrThrow(time, frame_id); + map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) + ->AddOdometerPose(time, odometry.pose * sensor_to_tracking.inverse(), + applied_covariance); + } catch (const tf2::TransformException& ex) { + LOG(WARNING) << "Cannot transform " << frame_id << " -> " + << options_.tracking_frame << ": " << ex.what(); + } } void Node::AddImu(const int64 timestamp, const string& frame_id, @@ -315,7 +323,7 @@ void Node::AddImu(const int64 timestamp, const string& frame_id, LookupToTrackingTransformOrThrow(time, frame_id); CHECK(sensor_to_tracking.translation().norm() < 1e-5) << "The IMU frame must be colocated with the tracking frame. " - "Transforming linear accelaration into the tracking frame will " + "Transforming linear acceleration into the tracking frame will " "otherwise be imprecise."; map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) ->AddImuData(time, @@ -328,6 +336,7 @@ void Node::AddImu(const int64 timestamp, const string& frame_id, << options_.tracking_frame << ": " << ex.what(); } } + void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, const proto::LaserScan& laser_scan) { const carto::common::Time time = carto::common::FromUniversal(timestamp); @@ -713,8 +722,7 @@ void Node::HandleSensorData(const int64 timestamp, return; case SensorType::kOdometry: - AddOdometry(timestamp, sensor_data->frame_id, sensor_data->odometry.pose, - sensor_data->odometry.covariance); + AddOdometry(timestamp, sensor_data->frame_id, sensor_data->odometry); return; } LOG(FATAL); diff --git a/cartographer_ros/src/sensor_data.cc b/cartographer_ros/src/sensor_data.cc index 064029c..dd5e765 100644 --- a/cartographer_ros/src/sensor_data.cc +++ b/cartographer_ros/src/sensor_data.cc @@ -53,10 +53,7 @@ SensorData::SensorData(const string& frame_id, proto::LaserFan3D laser_fan_3d) frame_id(CheckNoLeadingSlash(frame_id)), laser_fan_3d(laser_fan_3d) {} -SensorData::SensorData(const string& frame_id, const Rigid3d& pose, - const PoseCovariance& covariance) - : type(SensorType::kOdometry), - frame_id(frame_id), - odometry{pose, covariance} {} +SensorData::SensorData(const string& frame_id, const Odometry& odometry) + : type(SensorType::kOdometry), frame_id(frame_id), odometry(odometry) {} } // namespace cartorapher_ros diff --git a/cartographer_ros/src/sensor_data.h b/cartographer_ros/src/sensor_data.h index 0722698..c0a7527 100644 --- a/cartographer_ros/src/sensor_data.h +++ b/cartographer_ros/src/sensor_data.h @@ -29,24 +29,24 @@ namespace cartographer_ros { // is only used for time ordering sensor data before passing it on. enum class SensorType { kImu, kLaserScan, kLaserFan3D, kOdometry }; struct SensorData { + struct Odometry { + ::cartographer::transform::Rigid3d pose; + ::cartographer::kalman_filter::PoseCovariance covariance; + }; + SensorData(const string& frame_id, ::cartographer::sensor::proto::Imu imu); SensorData(const string& frame_id, ::cartographer::sensor::proto::LaserScan laser_scan); SensorData(const string& frame_id, ::cartographer::sensor::proto::LaserFan3D laser_fan_3d); - SensorData(const string& frame_id, - const ::cartographer::transform::Rigid3d& pose, - const ::cartographer::kalman_filter::PoseCovariance& covariance); + SensorData(const string& frame_id, const Odometry& odometry); SensorType type; string frame_id; ::cartographer::sensor::proto::Imu imu; ::cartographer::sensor::proto::LaserScan laser_scan; ::cartographer::sensor::proto::LaserFan3D laser_fan_3d; - struct { - ::cartographer::transform::Rigid3d pose; - ::cartographer::kalman_filter::PoseCovariance covariance; - } odometry; + Odometry odometry; }; } // namespace cartographer_ros diff --git a/cartographer_ros/src/sensor_data_producer.cc b/cartographer_ros/src/sensor_data_producer.cc index a6896b0..2a67ba2 100644 --- a/cartographer_ros/src/sensor_data_producer.cc +++ b/cartographer_ros/src/sensor_data_producer.cc @@ -29,8 +29,9 @@ SensorDataProducer::SensorDataProducer( void SensorDataProducer::AddOdometryMessage( const string& topic, const nav_msgs::Odometry::ConstPtr& msg) { auto sensor_data = ::cartographer::common::make_unique( - msg->header.frame_id, ToRigid3d(msg->pose.pose), - ToPoseCovariance(msg->pose.covariance)); + msg->child_frame_id, + SensorData::Odometry{ToRigid3d(msg->pose.pose), + ToPoseCovariance(msg->pose.covariance)}); sensor_collator_->AddSensorData( trajectory_id_, ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)), topic,