12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Adding NavSatFix to trajectory builder. (#666) 

GPS message is converted first to ECEF, and then to a local frame. The first GPS message defines the local frame. 

PAIR=wohe

[RFC=0007](https://github.com/googlecartographer/rfcs/blob/master/text/0007-nav-sat-support.md)
master
Susanne Pielawa 2018-01-12 19:35:16 +01:00 committed by Wally B. Feed
parent b0a937b7ed
commit 1eda46a941
2 changed files with 45 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -48,37 +48,57 @@ SensorBridge::SensorBridge(
tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer), tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
trajectory_builder_(trajectory_builder) {} trajectory_builder_(trajectory_builder) {}
std::unique_ptr<::cartographer::sensor::OdometryData> std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
SensorBridge::ToOdometryData(const nav_msgs::Odometry::ConstPtr& msg) { const nav_msgs::Odometry::ConstPtr& msg) {
const carto::common::Time time = FromRos(msg->header.stamp); const carto::common::Time time = FromRos(msg->header.stamp);
const auto sensor_to_tracking = tf_bridge_.LookupToTracking( const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
time, CheckNoLeadingSlash(msg->child_frame_id)); time, CheckNoLeadingSlash(msg->child_frame_id));
if (sensor_to_tracking == nullptr) { if (sensor_to_tracking == nullptr) {
return nullptr; return nullptr;
} }
return ::cartographer::common::make_unique< return carto::common::make_unique<carto::sensor::OdometryData>(
::cartographer::sensor::OdometryData>( carto::sensor::OdometryData{
::cartographer::sensor::OdometryData{
time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()}); time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
} }
void SensorBridge::HandleOdometryMessage( void SensorBridge::HandleOdometryMessage(
const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data = std::unique_ptr<carto::sensor::OdometryData> odometry_data =
ToOdometryData(msg); ToOdometryData(msg);
if (odometry_data != nullptr) { if (odometry_data != nullptr) {
trajectory_builder_->AddSensorData( trajectory_builder_->AddSensorData(
sensor_id, cartographer::sensor::OdometryData{odometry_data->time, sensor_id,
odometry_data->pose}); carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
} }
} }
void SensorBridge::HandleNavSatFixMessage( void SensorBridge::HandleNavSatFixMessage(
const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) { 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<Rigid3d>()});
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>(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<carto::sensor::ImuData> SensorBridge::ToImuData(
const sensor_msgs::Imu::ConstPtr& msg) { const sensor_msgs::Imu::ConstPtr& msg) {
CHECK_NE(msg->linear_acceleration_covariance[0], -1) CHECK_NE(msg->linear_acceleration_covariance[0], -1)
<< "Your IMU data claims to not contain linear acceleration measurements " << "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. " << "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.";
return ::cartographer::common::make_unique<::cartographer::sensor::ImuData>( return carto::common::make_unique<carto::sensor::ImuData>(
::cartographer::sensor::ImuData{ carto::sensor::ImuData{
time, time,
sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration), sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)}); 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, void SensorBridge::HandleImuMessage(const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) { const sensor_msgs::Imu::ConstPtr& msg) {
std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg); std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
if (imu_data != nullptr) { if (imu_data != nullptr) {
trajectory_builder_->AddSensorData( trajectory_builder_->AddSensorData(
sensor_id, cartographer::sensor::ImuData{imu_data->time, sensor_id,
imu_data->linear_acceleration, carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
imu_data->angular_velocity}); imu_data->angular_velocity});
} }
} }
void SensorBridge::HandleLaserScanMessage( void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
::cartographer::sensor::PointCloudWithIntensities point_cloud; carto::sensor::PointCloudWithIntensities point_cloud;
::cartographer::common::Time time; carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud); HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
} }
@ -130,8 +150,8 @@ void SensorBridge::HandleLaserScanMessage(
void SensorBridge::HandleMultiEchoLaserScanMessage( void SensorBridge::HandleMultiEchoLaserScanMessage(
const std::string& sensor_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
::cartographer::sensor::PointCloudWithIntensities point_cloud; carto::sensor::PointCloudWithIntensities point_cloud;
::cartographer::common::Time time; carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg); std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud); HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
} }
@ -187,7 +207,7 @@ void SensorBridge::HandleRangefinder(
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddSensorData( trajectory_builder_->AddSensorData(
sensor_id, cartographer::sensor::TimedPointCloudData{ sensor_id, carto::sensor::TimedPointCloudData{
time, sensor_to_tracking->translation().cast<float>(), time, sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformTimedPointCloud( carto::sensor::TransformTimedPointCloud(
ranges, sensor_to_tracking->cast<float>())}); ranges, sensor_to_tracking->cast<float>())});

4
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -19,6 +19,7 @@
#include <memory> #include <memory>
#include "cartographer/common/optional.h"
#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/odometry_data.h"
@ -82,6 +83,9 @@ class SensorBridge {
const TfBridge tf_bridge_; const TfBridge tf_bridge_;
::cartographer::mapping::TrajectoryBuilderInterface* const ::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_; trajectory_builder_;
::cartographer::common::optional<::cartographer::transform::Rigid3d>
ecef_to_local_frame_;
}; };
} // namespace cartographer_ros } // namespace cartographer_ros