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

Follow googlecartographer/cartographer/pull/724. (#616)

master
gaschler 2017-12-04 23:32:34 +01:00 committed by Christoph Schütte
parent e1f65f3104
commit 4dac7c3ebe
2 changed files with 26 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -65,26 +65,7 @@ void PushAndResetLineMarker(visualization_msgs::Marker* marker,
MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options, MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
tf2_ros::Buffer* const tf_buffer) tf2_ros::Buffer* const tf_buffer)
: node_options_(node_options), : node_options_(node_options),
map_builder_( map_builder_(node_options.map_builder_options),
node_options.map_builder_options,
cartographer::mapping::MapBuilder::LocalSlamResultCallback(
[this](
const int trajectory_id,
const ::cartographer::common::Time time,
const ::cartographer::transform::Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<const ::cartographer::mapping::NodeId>)
EXCLUDES(mutex_) {
std::shared_ptr<const TrajectoryState::LocalSlamData>
local_slam_data =
std::make_shared<TrajectoryState::LocalSlamData>(
TrajectoryState::LocalSlamData{
time, local_pose,
std::move(range_data_in_local)});
cartographer::common::MutexLocker lock(&mutex_);
trajectory_state_data_[trajectory_id] =
std::move(local_slam_data);
})),
tf_buffer_(tf_buffer) {} tf_buffer_(tf_buffer) {}
void MapBuilderBridge::LoadMap(const std::string& map_filename) { void MapBuilderBridge::LoadMap(const std::string& map_filename) {
@ -97,7 +78,11 @@ int MapBuilderBridge::AddTrajectory(
const std::unordered_set<std::string>& expected_sensor_ids, const std::unordered_set<std::string>& expected_sensor_ids,
const TrajectoryOptions& trajectory_options) { const TrajectoryOptions& trajectory_options) {
const int trajectory_id = map_builder_.AddTrajectoryBuilder( const int trajectory_id = map_builder_.AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options); expected_sensor_ids, trajectory_options.trajectory_builder_options,
::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,
::std::placeholders::_1, ::std::placeholders::_2,
::std::placeholders::_3, ::std::placeholders::_4,
::std::placeholders::_5));
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'."; LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
// Make sure there is no trajectory with 'trajectory_id' yet. // Make sure there is no trajectory with 'trajectory_id' yet.
@ -338,4 +323,17 @@ SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
return sensor_bridges_.at(trajectory_id).get(); return sensor_bridges_.at(trajectory_id).get();
} }
void MapBuilderBridge::OnLocalSlamResult(
const int trajectory_id, const ::cartographer::common::Time time,
const ::cartographer::transform::Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<const ::cartographer::mapping::NodeId>) {
std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
std::make_shared<TrajectoryState::LocalSlamData>(
TrajectoryState::LocalSlamData{time, local_pose,
std::move(range_data_in_local)});
cartographer::common::MutexLocker lock(&mutex_);
trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
}
} // namespace cartographer_ros } // namespace cartographer_ros

7
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -78,6 +78,13 @@ class MapBuilderBridge {
SensorBridge* sensor_bridge(int trajectory_id); SensorBridge* sensor_bridge(int trajectory_id);
private: private:
void OnLocalSlamResult(
const int trajectory_id, const ::cartographer::common::Time time,
const ::cartographer::transform::Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<const ::cartographer::mapping::NodeId>)
EXCLUDES(mutex_);
cartographer::common::Mutex mutex_; cartographer::common::Mutex mutex_;
const NodeOptions node_options_; const NodeOptions node_options_;
std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>> std::unordered_map<int, std::shared_ptr<const TrajectoryState::LocalSlamData>>