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'.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 1);
map_builder_.FinishTrajectory(trajectory_id);
map_builder_.sparse_pose_graph()->RunFinalOptimization();
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) {
cartographer::io::ProtoStreamWriter writer(filename);
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,
const TrajectoryOptions& trajectory_options);
void FinishTrajectory(int trajectory_id);
void RunFinalOptimization();
void SerializeState(const string& filename);
bool HandleSubmapQuery(

25
cartographer_ros/cartographer_ros/node.cc
View File

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