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

Support multiple simultaneous trajectories. (#194)

master
Damon Kohler 2016-11-28 12:52:34 +01:00 committed by GitHub
parent 43c4bacde3
commit 0d1e248f5f
4 changed files with 98 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -23,22 +23,50 @@
namespace cartographer_ros { namespace cartographer_ros {
MapBuilderBridge::MapBuilderBridge( MapBuilderBridge::MapBuilderBridge(const NodeOptions& options,
const NodeOptions& options, tf2_ros::Buffer* const tf_buffer)
const std::unordered_set<string>& expected_sensor_ids,
tf2_ros::Buffer* const tf_buffer)
: options_(options), : options_(options),
map_builder_(options.map_builder_options, &constant_data_), map_builder_(options.map_builder_options, &constant_data_),
expected_sensor_ids_(expected_sensor_ids), tf_buffer_(tf_buffer) {}
trajectory_id_(map_builder_.AddTrajectoryBuilder(expected_sensor_ids_)),
tf_bridge_(options_.tracking_frame, options_.lookup_transform_timeout_sec, int MapBuilderBridge::AddTrajectory(
tf_buffer) { const std::unordered_set<string>& expected_sensor_ids,
sensor_bridge_ = cartographer::common::make_unique<SensorBridge>( const string& tracking_frame) {
&tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_)); const int trajectory_id =
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),
map_builder_.GetTrajectoryBuilder(trajectory_id));
return trajectory_id;
} }
MapBuilderBridge::~MapBuilderBridge() { void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
map_builder_.FinishTrajectory(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);
}
void MapBuilderBridge::WriteAssets(const string& stem) {
const auto trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
if (trajectory_nodes.empty()) {
LOG(WARNING) << "No data was collected and no assets will be written.";
} else {
LOG(INFO) << "Writing assets to '" << stem << "'...";
cartographer_ros::WriteAssets(trajectory_nodes, options_, stem);
}
} }
bool MapBuilderBridge::HandleSubmapQuery( bool MapBuilderBridge::HandleSubmapQuery(
@ -63,32 +91,6 @@ bool MapBuilderBridge::HandleSubmapQuery(
return true; return true;
} }
bool MapBuilderBridge::HandleFinishTrajectory(
cartographer_ros_msgs::FinishTrajectory::Request& request,
cartographer_ros_msgs::FinishTrajectory::Response& response) {
LOG(INFO) << "Finishing trajectory...";
const int previous_trajectory_id = trajectory_id_;
trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
sensor_bridge_ = cartographer::common::make_unique<SensorBridge>(
&tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
map_builder_.FinishTrajectory(previous_trajectory_id);
map_builder_.sparse_pose_graph()->RunFinalOptimization();
const auto trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
if (trajectory_nodes.empty()) {
LOG(WARNING) << "No data collected and no assets will be written.";
} else {
LOG(INFO) << "Writing assets...";
WriteAssets(trajectory_nodes, options_, request.stem);
}
LOG(INFO) << "New trajectory started.";
return true;
}
cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
cartographer_ros_msgs::SubmapList submap_list; cartographer_ros_msgs::SubmapList submap_list;
submap_list.header.stamp = ::ros::Time::now(); submap_list.header.stamp = ::ros::Time::now();
@ -130,4 +132,16 @@ MapBuilderBridge::BuildOccupancyGrid() {
return occupancy_grid; return occupancy_grid;
} }
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_;
}
} // namespace cartographer_ros } // namespace cartographer_ros

30
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -19,6 +19,7 @@
#include <memory> #include <memory>
#include <unordered_set> #include <unordered_set>
#include <unordered_map>
#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/node_options.h" #include "cartographer_ros/node_options.h"
@ -34,30 +35,26 @@ namespace cartographer_ros {
class MapBuilderBridge { class MapBuilderBridge {
public: public:
MapBuilderBridge(const NodeOptions& options, MapBuilderBridge(const NodeOptions& options, tf2_ros::Buffer* tf_buffer);
const std::unordered_set<string>& expected_sensor_ids,
tf2_ros::Buffer* tf_buffer);
~MapBuilderBridge();
MapBuilderBridge(const MapBuilderBridge&) = delete; MapBuilderBridge(const MapBuilderBridge&) = delete;
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
const string& tracking_frame);
void FinishTrajectory(int trajectory_id);
void WriteAssets(const string& stem);
bool HandleSubmapQuery( bool HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response); cartographer_ros_msgs::SubmapQuery::Response& response);
bool HandleFinishTrajectory(
cartographer_ros_msgs::FinishTrajectory::Request& request,
cartographer_ros_msgs::FinishTrajectory::Response& response);
cartographer_ros_msgs::SubmapList GetSubmapList(); cartographer_ros_msgs::SubmapList GetSubmapList();
std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid(); std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid();
SensorBridge* sensor_bridge() { return sensor_bridge_.get(); } SensorBridge* sensor_bridge(int trajectory_id);
TfBridge* tf_bridge(int trajectory_id);
// TODO(damonkohler): Remove these accessors. cartographer::mapping::MapBuilder* map_builder();
int trajectory_id() const { return trajectory_id_; }
TfBridge* tf_bridge() { return &tf_bridge_; }
cartographer::mapping::MapBuilder* map_builder() { return &map_builder_; }
private: private:
const NodeOptions options_; const NodeOptions options_;
@ -65,10 +62,9 @@ class MapBuilderBridge {
std::deque<cartographer::mapping::TrajectoryNode::ConstantData> std::deque<cartographer::mapping::TrajectoryNode::ConstantData>
constant_data_; constant_data_;
cartographer::mapping::MapBuilder map_builder_; cartographer::mapping::MapBuilder map_builder_;
std::unordered_set<string> expected_sensor_ids_; tf2_ros::Buffer* const tf_buffer_;
int trajectory_id_ = -1; std::unordered_map<int, std::unique_ptr<TfBridge>> tf_bridges_;
TfBridge tf_bridge_; std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
std::unique_ptr<SensorBridge> sensor_bridge_;
}; };
} // namespace cartographer_ros } // namespace cartographer_ros

55
cartographer_ros/cartographer_ros/node.cc
View File

@ -64,11 +64,14 @@ constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
Node::Node(const NodeOptions& options) Node::Node(const NodeOptions& options)
: options_(options), : options_(options),
tf_buffer_(::ros::Duration(kTfBufferCacheTimeInSeconds)), tf_buffer_(::ros::Duration(kTfBufferCacheTimeInSeconds)),
tf_(tf_buffer_) {} tf_(tf_buffer_),
map_builder_bridge_(options_, &tf_buffer_) {}
Node::~Node() { Node::~Node() {
{ {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
CHECK_GE(trajectory_id_, 0);
map_builder_bridge_.FinishTrajectory(trajectory_id_);
terminating_ = true; terminating_ = true;
} }
if (occupancy_grid_thread_.joinable()) { if (occupancy_grid_thread_.joinable()) {
@ -78,29 +81,27 @@ Node::~Node() {
void Node::Initialize() { void Node::Initialize() {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
std::unordered_set<string> expected_sensor_ids;
// For 2D SLAM, subscribe to exactly one horizontal laser. // For 2D SLAM, subscribe to exactly one horizontal laser.
if (options_.use_laser_scan) { if (options_.use_laser_scan) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe( horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kLaserScanTopic, kInfiniteSubscriberQueueSize, kLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>( boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
[this](const sensor_msgs::LaserScan::ConstPtr& msg) { [this](const sensor_msgs::LaserScan::ConstPtr& msg) {
map_builder_bridge_->sensor_bridge()->HandleLaserScanMessage( map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleLaserScanMessage(
kLaserScanTopic, msg); kLaserScanTopic, msg);
})); }));
expected_sensor_ids.insert(kLaserScanTopic); expected_sensor_ids_.insert(kLaserScanTopic);
} }
if (options_.use_multi_echo_laser_scan) { if (options_.use_multi_echo_laser_scan) {
horizontal_laser_scan_subscriber_ = node_handle_.subscribe( horizontal_laser_scan_subscriber_ = node_handle_.subscribe(
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize, kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>( boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { [this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
map_builder_bridge_->sensor_bridge() map_builder_bridge_.sensor_bridge(trajectory_id_)
->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic, ->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic,
msg); msg);
})); }));
expected_sensor_ids.insert(kMultiEchoLaserScanTopic); expected_sensor_ids_.insert(kMultiEchoLaserScanTopic);
} }
// For 3D SLAM, subscribe to all point clouds topics. // For 3D SLAM, subscribe to all point clouds topics.
@ -114,10 +115,10 @@ void Node::Initialize() {
topic, kInfiniteSubscriberQueueSize, topic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>( boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { [this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
map_builder_bridge_->sensor_bridge()->HandlePointCloud2Message( map_builder_bridge_.sensor_bridge(trajectory_id_)->HandlePointCloud2Message(
topic, msg); topic, msg);
}))); })));
expected_sensor_ids.insert(topic); expected_sensor_ids_.insert(topic);
} }
} }
@ -131,10 +132,10 @@ void Node::Initialize() {
kImuTopic, kInfiniteSubscriberQueueSize, kImuTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>( boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
[this](const sensor_msgs::Imu::ConstPtr& msg) { [this](const sensor_msgs::Imu::ConstPtr& msg) {
map_builder_bridge_->sensor_bridge()->HandleImuMessage(kImuTopic, map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleImuMessage(kImuTopic,
msg); msg);
})); }));
expected_sensor_ids.insert(kImuTopic); expected_sensor_ids_.insert(kImuTopic);
} }
if (options_.use_odometry) { if (options_.use_odometry) {
@ -142,14 +143,14 @@ void Node::Initialize() {
kOdometryTopic, kInfiniteSubscriberQueueSize, kOdometryTopic, kInfiniteSubscriberQueueSize,
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>( boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
[this](const nav_msgs::Odometry::ConstPtr& msg) { [this](const nav_msgs::Odometry::ConstPtr& msg) {
map_builder_bridge_->sensor_bridge()->HandleOdometryMessage( map_builder_bridge_.sensor_bridge(trajectory_id_)->HandleOdometryMessage(
kOdometryTopic, msg); kOdometryTopic, msg);
})); }));
expected_sensor_ids.insert(kOdometryTopic); expected_sensor_ids_.insert(kOdometryTopic);
} }
map_builder_bridge_ = carto::common::make_unique<MapBuilderBridge>( trajectory_id_ = map_builder_bridge_.AddTrajectory(expected_sensor_ids_,
options_, expected_sensor_ids, &tf_buffer_); options_.tracking_frame);
submap_list_publisher_ = submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>( node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
@ -185,27 +186,31 @@ bool Node::HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response) { ::cartographer_ros_msgs::SubmapQuery::Response& response) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
return map_builder_bridge_->HandleSubmapQuery(request, response); return map_builder_bridge_.HandleSubmapQuery(request, response);
} }
bool Node::HandleFinishTrajectory( bool Node::HandleFinishTrajectory(
::cartographer_ros_msgs::FinishTrajectory::Request& request, ::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response& response) { ::cartographer_ros_msgs::FinishTrajectory::Response&) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
return map_builder_bridge_->HandleFinishTrajectory(request, response); const int previous_trajectory_id = trajectory_id_;
trajectory_id_ = map_builder_bridge_.AddTrajectory(expected_sensor_ids_,
options_.tracking_frame);
map_builder_bridge_.FinishTrajectory(previous_trajectory_id);
map_builder_bridge_.WriteAssets(request.stem);
return true;
} }
void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
submap_list_publisher_.publish(map_builder_bridge_->GetSubmapList()); submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
} }
void Node::PublishPoseAndScanMatchedPointCloud( void Node::PublishPoseAndScanMatchedPointCloud(
const ::ros::WallTimerEvent& timer_event) { const ::ros::WallTimerEvent& timer_event) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
const carto::mapping::TrajectoryBuilder* trajectory_builder = const carto::mapping::TrajectoryBuilder* trajectory_builder =
map_builder_bridge_->map_builder()->GetTrajectoryBuilder( map_builder_bridge_.map_builder()->GetTrajectoryBuilder(trajectory_id_);
map_builder_bridge_->trajectory_id());
const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate = const carto::mapping::TrajectoryBuilder::PoseEstimate last_pose_estimate =
trajectory_builder->pose_estimate(); trajectory_builder->pose_estimate();
if (carto::common::ToUniversal(last_pose_estimate.time) < 0) { if (carto::common::ToUniversal(last_pose_estimate.time) < 0) {
@ -214,7 +219,7 @@ void Node::PublishPoseAndScanMatchedPointCloud(
const Rigid3d tracking_to_local = last_pose_estimate.pose; const Rigid3d tracking_to_local = last_pose_estimate.pose;
const Rigid3d local_to_map = const Rigid3d local_to_map =
map_builder_bridge_->map_builder() map_builder_bridge_.map_builder()
->sparse_pose_graph() ->sparse_pose_graph()
->GetLocalToGlobalTransform(*trajectory_builder->submaps()); ->GetLocalToGlobalTransform(*trajectory_builder->submaps());
const Rigid3d tracking_to_map = local_to_map * tracking_to_local; const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
@ -239,8 +244,8 @@ void Node::PublishPoseAndScanMatchedPointCloud(
} }
const auto published_to_tracking = const auto published_to_tracking =
map_builder_bridge_->tf_bridge()->LookupToTracking( map_builder_bridge_.tf_bridge(trajectory_id_)
last_pose_estimate.time, options_.published_frame); ->LookupToTracking(last_pose_estimate.time, options_.published_frame);
if (published_to_tracking != nullptr) { if (published_to_tracking != nullptr) {
if (options_.provide_odom_frame) { if (options_.provide_odom_frame) {
std::vector<geometry_msgs::TransformStamped> stamped_transforms; std::vector<geometry_msgs::TransformStamped> stamped_transforms;
@ -279,7 +284,7 @@ void Node::SpinOccupancyGridThreadForever() {
if (occupancy_grid_publisher_.getNumSubscribers() == 0) { if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
continue; continue;
} }
const auto occupancy_grid = map_builder_bridge_->BuildOccupancyGrid(); const auto occupancy_grid = map_builder_bridge_.BuildOccupancyGrid();
if (occupancy_grid != nullptr) { if (occupancy_grid != nullptr) {
occupancy_grid_publisher_.publish(*occupancy_grid); occupancy_grid_publisher_.publish(*occupancy_grid);
} }

4
cartographer_ros/cartographer_ros/node.h
View File

@ -66,7 +66,9 @@ class Node {
tf2_ros::TransformBroadcaster tf_broadcaster_; tf2_ros::TransformBroadcaster tf_broadcaster_;
cartographer::common::Mutex mutex_; cartographer::common::Mutex mutex_;
std::unique_ptr<MapBuilderBridge> map_builder_bridge_ GUARDED_BY(mutex_); MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
int trajectory_id_ = -1;
std::unordered_set<string> expected_sensor_ids_;
::ros::NodeHandle node_handle_; ::ros::NodeHandle node_handle_;
::ros::Subscriber imu_subscriber_; ::ros::Subscriber imu_subscriber_;