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

Release mutex in Node during final optimization (#480) 

Enables servicing callbacks during final optimization, thus
enabling visualization of its progress in RViz. Fixes #476.
master
Juraj Oršulić 2017-08-10 09:55:52 +02:00 committed by Damon Kohler
parent 4173beaf04
commit 102a3b0db3
3 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -77,10 +77,14 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
// Make sure there is a trajectory with 'trajectory_id'. // Make sure there is a trajectory with 'trajectory_id'.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 1); CHECK_EQ(sensor_bridges_.count(trajectory_id), 1);
map_builder_.FinishTrajectory(trajectory_id); map_builder_.FinishTrajectory(trajectory_id);
map_builder_.sparse_pose_graph()->RunFinalOptimization();
sensor_bridges_.erase(trajectory_id); sensor_bridges_.erase(trajectory_id);
} }
void MapBuilderBridge::RunFinalOptimization() {
LOG(INFO) << "Running final trajectory optimization...";
map_builder_.sparse_pose_graph()->RunFinalOptimization();
}
void MapBuilderBridge::SerializeState(const std::string& filename) { void MapBuilderBridge::SerializeState(const std::string& filename) {
cartographer::io::ProtoStreamWriter writer(filename); cartographer::io::ProtoStreamWriter writer(filename);
map_builder_.SerializeState(&writer); map_builder_.SerializeState(&writer);

1
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -54,6 +54,7 @@ class MapBuilderBridge {
int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids, int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
const TrajectoryOptions& trajectory_options); const TrajectoryOptions& trajectory_options);
void FinishTrajectory(int trajectory_id); void FinishTrajectory(int trajectory_id);
void RunFinalOptimization();
void SerializeState(const string& filename); void SerializeState(const string& filename);
bool HandleSubmapQuery( bool HandleSubmapQuery(

9
cartographer_ros/cartographer_ros/node.cc
View File

@ -428,21 +428,28 @@ bool Node::HandleWriteState(
} }
void Node::FinishAllTrajectories() { void Node::FinishAllTrajectories() {
{
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
for (const auto& entry : is_active_trajectory_) { for (auto& entry : is_active_trajectory_) {
const int trajectory_id = entry.first; const int trajectory_id = entry.first;
if (entry.second) { if (entry.second) {
map_builder_bridge_.FinishTrajectory(trajectory_id); map_builder_bridge_.FinishTrajectory(trajectory_id);
entry.second = false;
} }
} }
} }
map_builder_bridge_.RunFinalOptimization();
}
void Node::FinishTrajectory(const int trajectory_id) { void Node::FinishTrajectory(const int trajectory_id) {
{
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
CHECK(is_active_trajectory_.at(trajectory_id)); CHECK(is_active_trajectory_.at(trajectory_id));
map_builder_bridge_.FinishTrajectory(trajectory_id); map_builder_bridge_.FinishTrajectory(trajectory_id);
is_active_trajectory_[trajectory_id] = false; is_active_trajectory_[trajectory_id] = false;
} }
map_builder_bridge_.RunFinalOptimization();
}
void Node::HandleOdometryMessage(const int trajectory_id, void Node::HandleOdometryMessage(const int trajectory_id,
const string& sensor_id, const string& sensor_id,