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

Unify finish trajectory logic (#552) 

Port active trajectory checks and subscriber shutdown logic into FinishTrajectory(), so that a trajectory can be cleanly finished by function call.
master
Jihoon Lee 2017-11-14 11:30:39 +01:00 committed by Wally B. Feed
parent 3e684b298b
commit 9fb2b09a0b
2 changed files with 32 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
cartographer_ros/cartographer_ros/node.cc
View File

@ -375,6 +375,30 @@ bool Node::ValidateTopicNames(
return true;
}
bool Node::FinishTrajectoryUnderLock(const int trajectory_id) {
if (is_active_trajectory_.count(trajectory_id) == 0) {
LOG(INFO) << "Trajectory_id " << trajectory_id << " is not created yet.";
return false;
}
if (!is_active_trajectory_[trajectory_id]) {
LOG(INFO) << "Trajectory_id " << trajectory_id
<< " has already been finished.";
return false;
}
// Shutdown the subscribers of this trajectory.
for (auto& entry : subscribers_[trajectory_id]) {
entry.subscriber.shutdown();
subscribed_topics_.erase(entry.topic);
LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
}
CHECK_EQ(subscribers_.erase(trajectory_id), 1);
CHECK(is_active_trajectory_.at(trajectory_id));
map_builder_bridge_.FinishTrajectory(trajectory_id);
is_active_trajectory_[trajectory_id] = false;
return true;
}
bool Node::HandleStartTrajectory(
::cartographer_ros_msgs::StartTrajectory::Request& request,
::cartographer_ros_msgs::StartTrajectory::Response& response) {
@ -421,27 +445,7 @@ bool Node::HandleFinishTrajectory(
::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response& response) {
carto::common::MutexLocker lock(&mutex_);
const int trajectory_id = request.trajectory_id;
if (is_active_trajectory_.count(trajectory_id) == 0) {
LOG(INFO) << "Trajectory_id " << trajectory_id << " is not created yet.";
return false;
}
if (!is_active_trajectory_[trajectory_id]) {
LOG(INFO) << "Trajectory_id " << trajectory_id
<< " has already been finished.";
return false;
}
// Shutdown the subscribers of this trajectory.
for (auto& entry : subscribers_[trajectory_id]) {
entry.subscriber.shutdown();
subscribed_topics_.erase(entry.topic);
LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
}
CHECK_EQ(subscribers_.erase(trajectory_id), 1);
map_builder_bridge_.FinishTrajectory(trajectory_id);
is_active_trajectory_[trajectory_id] = false;
return true;
return FinishTrajectoryUnderLock(request.trajectory_id);
}
bool Node::HandleWriteState(
@ -457,17 +461,14 @@ void Node::FinishAllTrajectories() {
for (auto& entry : is_active_trajectory_) {
const int trajectory_id = entry.first;
if (entry.second) {
map_builder_bridge_.FinishTrajectory(trajectory_id);
entry.second = false;
CHECK(FinishTrajectoryUnderLock(trajectory_id));
}
}
}
void Node::FinishTrajectory(const int trajectory_id) {
bool 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;
return FinishTrajectoryUnderLock(trajectory_id);
}
void Node::RunFinalOptimization() {

6
cartographer_ros/cartographer_ros/node.h
View File

@ -54,8 +54,9 @@ class Node {
// Finishes all yet active trajectories.
void FinishAllTrajectories();
// Finishes a single given trajectory.
void FinishTrajectory(int trajectory_id);
// Finishes a single given trajectory. Returns false if the trajectory did not
// exist or was already finished.
bool FinishTrajectory(int trajectory_id);
// Runs final optimization. All trajectories have to be finished when calling.
void RunFinalOptimization();
@ -134,6 +135,7 @@ class Node {
bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics,
const TrajectoryOptions& options);
bool FinishTrajectoryUnderLock(int trajectory_id) REQUIRES(mutex_);
const NodeOptions node_options_;