diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 55aa4f0..ad10674 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -48,37 +48,57 @@ SensorBridge::SensorBridge( tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer), trajectory_builder_(trajectory_builder) {} -std::unique_ptr<::cartographer::sensor::OdometryData> -SensorBridge::ToOdometryData(const nav_msgs::Odometry::ConstPtr& msg) { +std::unique_ptr SensorBridge::ToOdometryData( + const nav_msgs::Odometry::ConstPtr& msg) { const carto::common::Time time = FromRos(msg->header.stamp); const auto sensor_to_tracking = tf_bridge_.LookupToTracking( time, CheckNoLeadingSlash(msg->child_frame_id)); if (sensor_to_tracking == nullptr) { return nullptr; } - return ::cartographer::common::make_unique< - ::cartographer::sensor::OdometryData>( - ::cartographer::sensor::OdometryData{ + return carto::common::make_unique( + carto::sensor::OdometryData{ time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()}); } void SensorBridge::HandleOdometryMessage( const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { - std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data = + std::unique_ptr odometry_data = ToOdometryData(msg); if (odometry_data != nullptr) { trajectory_builder_->AddSensorData( - sensor_id, cartographer::sensor::OdometryData{odometry_data->time, - odometry_data->pose}); + sensor_id, + carto::sensor::OdometryData{odometry_data->time, odometry_data->pose}); } } void SensorBridge::HandleNavSatFixMessage( const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) { - LOG(FATAL) << "TODO(spielawa / wohe): NavSatFix support not yet implemented."; + const carto::common::Time time = FromRos(msg->header.stamp); + if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) { + trajectory_builder_->AddSensorData( + sensor_id, carto::sensor::FixedFramePoseData{ + time, carto::common::optional()}); + return; + } + + if (!ecef_to_local_frame_.has_value()) { + ecef_to_local_frame_ = + ComputeLocalFrameFromLatLong(msg->latitude, msg->longitude); + LOG(INFO) << "Using NavSatFix. Setting ecef_to_local_frame with lat = " + << msg->latitude << ", long = " << msg->longitude << "."; + } + + trajectory_builder_->AddSensorData( + sensor_id, + carto::sensor::FixedFramePoseData{ + time, carto::common::optional(Rigid3d::Translation( + ecef_to_local_frame_.value() * + LatLongAltToEcef(msg->latitude, msg->longitude, + msg->altitude)))}); } -std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData( +std::unique_ptr SensorBridge::ToImuData( const sensor_msgs::Imu::ConstPtr& msg) { CHECK_NE(msg->linear_acceleration_covariance[0], -1) << "Your IMU data claims to not contain linear acceleration measurements " @@ -101,8 +121,8 @@ std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData( << "The IMU frame must be colocated with the tracking frame. " "Transforming linear acceleration into the tracking frame will " "otherwise be imprecise."; - return ::cartographer::common::make_unique<::cartographer::sensor::ImuData>( - ::cartographer::sensor::ImuData{ + return carto::common::make_unique( + carto::sensor::ImuData{ time, sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration), sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)}); @@ -110,19 +130,19 @@ std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData( void SensorBridge::HandleImuMessage(const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg) { - std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg); + std::unique_ptr imu_data = ToImuData(msg); if (imu_data != nullptr) { trajectory_builder_->AddSensorData( - sensor_id, cartographer::sensor::ImuData{imu_data->time, - imu_data->linear_acceleration, - imu_data->angular_velocity}); + sensor_id, + carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration, + imu_data->angular_velocity}); } } void SensorBridge::HandleLaserScanMessage( const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { - ::cartographer::sensor::PointCloudWithIntensities point_cloud; - ::cartographer::common::Time time; + carto::sensor::PointCloudWithIntensities point_cloud; + carto::common::Time time; std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud); } @@ -130,8 +150,8 @@ void SensorBridge::HandleLaserScanMessage( void SensorBridge::HandleMultiEchoLaserScanMessage( const std::string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { - ::cartographer::sensor::PointCloudWithIntensities point_cloud; - ::cartographer::common::Time time; + carto::sensor::PointCloudWithIntensities point_cloud; + carto::common::Time time; std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud); } @@ -187,7 +207,7 @@ void SensorBridge::HandleRangefinder( tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); if (sensor_to_tracking != nullptr) { trajectory_builder_->AddSensorData( - sensor_id, cartographer::sensor::TimedPointCloudData{ + sensor_id, carto::sensor::TimedPointCloudData{ time, sensor_to_tracking->translation().cast(), carto::sensor::TransformTimedPointCloud( ranges, sensor_to_tracking->cast())}); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index a6e2e25..906c3e3 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -19,6 +19,7 @@ #include +#include "cartographer/common/optional.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" @@ -82,6 +83,9 @@ class SensorBridge { const TfBridge tf_bridge_; ::cartographer::mapping::TrajectoryBuilderInterface* const trajectory_builder_; + + ::cartographer::common::optional<::cartographer::transform::Rigid3d> + ecef_to_local_frame_; }; } // namespace cartographer_ros