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

Take frozen state into account when finishing trajectories. (#811) 

Until now, the error response of an /finish_trajectory request for a
frozen trajectory was 'Trajectory ... is not created yet.'.

This is a lie. The new response is more accurate because the trajectory
__is__ created, but it just can't be finished because it's frozen.
master
Michael Grupp 2018-04-09 18:59:34 +02:00 committed by Wally B. Feed
parent c0a97d88a3
commit a375153a21
3 changed files with 24 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -197,6 +197,17 @@ void MapBuilderBridge::HandleSubmapQuery(
response.status.code = cartographer_ros_msgs::StatusCode::OK;
}
std::set<int> MapBuilderBridge::GetFrozenTrajectoryIds() {
std::set<int> frozen_trajectory_ids;
const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();
for (const int trajectory_id : node_poses.trajectory_ids()) {
if (map_builder_->pose_graph()->IsTrajectoryFrozen(trajectory_id)) {
frozen_trajectory_ids.insert(trajectory_id);
}
}
return frozen_trajectory_ids;
}
cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
cartographer_ros_msgs::SubmapList submap_list;
submap_list.header.stamp = ::ros::Time::now();

1
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -77,6 +77,7 @@ class MapBuilderBridge {
cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response);
std::set<int> GetFrozenTrajectoryIds();
cartographer_ros_msgs::SubmapList GetSubmapList();
std::unordered_map<int, TrajectoryState> GetTrajectoryStates()
EXCLUDES(mutex_);

14
cartographer_ros/cartographer_ros/node.cc
View File

@ -443,10 +443,20 @@ bool Node::ValidateTopicNames(
cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
const int trajectory_id) {
cartographer_ros_msgs::StatusResponse status_response;
// First, check if we can actually finish the trajectory.
if (map_builder_bridge_.GetFrozenTrajectoryIds().count(trajectory_id)) {
const std::string error =
"Trajectory " + std::to_string(trajectory_id) + " is frozen.";
LOG(ERROR) << error;
status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
status_response.message = error;
return status_response;
}
if (is_active_trajectory_.count(trajectory_id) == 0) {
const std::string error =
"Trajectory " + std::to_string(trajectory_id) + " is not created yet.";
LOG(INFO) << error;
LOG(ERROR) << error;
status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
status_response.message = error;
return status_response;
@ -454,7 +464,7 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
if (!is_active_trajectory_[trajectory_id]) {
const std::string error = "Trajectory " + std::to_string(trajectory_id) +
" has already been finished.";
LOG(INFO) << error;
LOG(ERROR) << error;
status_response.code =
cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
status_response.message = error;