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

Moves TfBridge into SensorBridge. (#195)

master
Damon Kohler 2016-11-28 14:50:04 +01:00 committed by GitHub
parent 0d1e248f5f
commit 33271f0290
5 changed files with 18 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -36,13 +36,10 @@ int MapBuilderBridge::AddTrajectory(
map_builder_.AddTrajectoryBuilder(expected_sensor_ids);
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
CHECK_EQ(tf_bridges_.count(trajectory_id), 0);
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
tf_bridges_[trajectory_id] = cartographer::common::make_unique<TfBridge>(
tracking_frame, options_.lookup_transform_timeout_sec, tf_buffer_);
sensor_bridges_[trajectory_id] =
cartographer::common::make_unique<SensorBridge>(
tf_bridge(trajectory_id),
tracking_frame, options_.lookup_transform_timeout_sec, tf_buffer_,
map_builder_.GetTrajectoryBuilder(trajectory_id));
return trajectory_id;
}
@ -50,11 +47,9 @@ int MapBuilderBridge::AddTrajectory(
void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";
CHECK_EQ(tf_bridges_.count(trajectory_id), 1);
CHECK_EQ(sensor_bridges_.count(trajectory_id), 1);
map_builder_.FinishTrajectory(trajectory_id);
map_builder_.sparse_pose_graph()->RunFinalOptimization();
tf_bridges_.erase(trajectory_id);
sensor_bridges_.erase(trajectory_id);
}
@ -136,10 +131,6 @@ SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
return sensor_bridges_.at(trajectory_id).get();
}
TfBridge* MapBuilderBridge::tf_bridge(const int trajectory_id) {
return tf_bridges_.at(trajectory_id).get();
}
cartographer::mapping::MapBuilder* MapBuilderBridge::map_builder() {
return &map_builder_;
}

2
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -53,7 +53,6 @@ class MapBuilderBridge {
std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid();
SensorBridge* sensor_bridge(int trajectory_id);
TfBridge* tf_bridge(int trajectory_id);
cartographer::mapping::MapBuilder* map_builder();
private:
@ -63,7 +62,6 @@ class MapBuilderBridge {
constant_data_;
cartographer::mapping::MapBuilder map_builder_;
tf2_ros::Buffer* const tf_buffer_;
std::unordered_map<int, std::unique_ptr<TfBridge>> tf_bridges_;
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
};

5
cartographer_ros/cartographer_ros/node.cc
View File

@ -244,8 +244,9 @@ void Node::PublishPoseAndScanMatchedPointCloud(
}
const auto published_to_tracking =
map_builder_bridge_.tf_bridge(trajectory_id_)
->LookupToTracking(last_pose_estimate.time, options_.published_frame);
map_builder_bridge_.sensor_bridge(trajectory_id_)
->tf_bridge()
.LookupToTracking(last_pose_estimate.time, options_.published_frame);
if (published_to_tracking != nullptr) {
if (options_.provide_odom_frame) {
std::vector<geometry_msgs::TransformStamped> stamped_transforms;

14
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -38,14 +38,16 @@ const string& CheckNoLeadingSlash(const string& frame_id) {
} // namespace
SensorBridge::SensorBridge(
const TfBridge* const tf_bridge,
const string& tracking_frame, const double lookup_transform_timeout_sec,
tf2_ros::Buffer* const tf_buffer,
carto::mapping::TrajectoryBuilder* const trajectory_builder)
: tf_bridge_(tf_bridge), trajectory_builder_(trajectory_builder) {}
: tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
trajectory_builder_(trajectory_builder) {}
void SensorBridge::HandleOdometryMessage(
const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
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));
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddOdometerData(
@ -59,7 +61,7 @@ void SensorBridge::HandleImuMessage(const string& sensor_id,
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(
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)
@ -98,12 +100,14 @@ void SensorBridge::HandlePointCloud2Message(
point_cloud);
}
const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
void SensorBridge::HandleRangefinder(const string& sensor_id,
const carto::common::Time time,
const string& frame_id,
const carto::sensor::PointCloud& ranges) {
const auto sensor_to_tracking =
tf_bridge_->LookupToTracking(time, CheckNoLeadingSlash(frame_id));
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddRangefinderData(
sensor_id, time, sensor_to_tracking->translation().cast<float>(),

7
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -36,7 +36,8 @@ namespace cartographer_ros {
class SensorBridge {
public:
explicit SensorBridge(
const TfBridge* tf_bridge,
const string& tracking_frame, double lookup_transform_timeout_sec,
tf2_ros::Buffer* tf_buffer,
::cartographer::mapping::TrajectoryBuilder* trajectory_builder);
SensorBridge(const SensorBridge&) = delete;
@ -54,13 +55,15 @@ class SensorBridge {
void HandlePointCloud2Message(const string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg);
const TfBridge& tf_bridge() const;
private:
void HandleRangefinder(const string& sensor_id,
const ::cartographer::common::Time time,
const string& frame_id,
const ::cartographer::sensor::PointCloud& ranges);
const TfBridge* const tf_bridge_;
const TfBridge tf_bridge_;
::cartographer::mapping::TrajectoryBuilder* const trajectory_builder_;
};