Use PoseGraphInterface::TrajectoryState from libcartographer (#910)

https://github.com/googlecartographer/rfcs/pull/35

- makes use of the trajectory state in `map_builder` and `node`
- adds a service to query the trajectory states
- follow-up to https://github.com/googlecartographer/cartographer/pull/1214
  that takes the deleted state into account in the `/finish_trajectory` service
  (could crash otherwise)
master
Michael Grupp 2018-06-29 11:14:56 +02:00 committed by gaschler
parent c560d05d99
commit 8bbec6bff7
8 changed files with 136 additions and 40 deletions

View File

@ -147,7 +147,7 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'..."; LOG(INFO) << "Finishing trajectory with ID '" << 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(GetTrajectoryStates().count(trajectory_id));
map_builder_->FinishTrajectory(trajectory_id); map_builder_->FinishTrajectory(trajectory_id);
sensor_bridges_.erase(trajectory_id); sensor_bridges_.erase(trajectory_id);
} }
@ -194,15 +194,9 @@ void MapBuilderBridge::HandleSubmapQuery(
response.status.code = cartographer_ros_msgs::StatusCode::OK; response.status.code = cartographer_ros_msgs::StatusCode::OK;
} }
std::set<int> MapBuilderBridge::GetFrozenTrajectoryIds() { std::map<int, ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
std::set<int> frozen_trajectory_ids; MapBuilderBridge::GetTrajectoryStates() {
const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses(); return map_builder_->pose_graph()->GetTrajectoryStates();
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 MapBuilderBridge::GetSubmapList() {

View File

@ -24,6 +24,7 @@
#include "cartographer/common/mutex.h" #include "cartographer/common/mutex.h"
#include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/mapping/trajectory_builder_interface.h"
#include "cartographer_ros/node_options.h" #include "cartographer_ros/node_options.h"
@ -77,7 +78,9 @@ class MapBuilderBridge {
cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response); cartographer_ros_msgs::SubmapQuery::Response& response);
std::set<int> GetFrozenTrajectoryIds(); std::map<int /* trajectory_id */,
::cartographer::mapping::PoseGraphInterface::TrajectoryState>
GetTrajectoryStates();
cartographer_ros_msgs::SubmapList GetSubmapList(); cartographer_ros_msgs::SubmapList GetSubmapList();
std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData() std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
EXCLUDES(mutex_); EXCLUDES(mutex_);

View File

@ -26,6 +26,7 @@
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/common/time.h" #include "cartographer/common/time.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
@ -81,6 +82,8 @@ template <typename MessageType>
namespace carto = ::cartographer; namespace carto = ::cartographer;
using carto::transform::Rigid3d; using carto::transform::Rigid3d;
using TrajectoryState =
::cartographer::mapping::PoseGraphInterface::TrajectoryState;
Node::Node( Node::Node(
const NodeOptions& node_options, const NodeOptions& node_options,
@ -109,6 +112,8 @@ Node::Node(
kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this)); kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService( service_servers_.push_back(node_handle_.advertiseService(
kWriteStateServiceName, &Node::HandleWriteState, this)); kWriteStateServiceName, &Node::HandleWriteState, this));
service_servers_.push_back(node_handle_.advertiseService(
kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
scan_matched_point_cloud_publisher_ = scan_matched_point_cloud_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>( node_handle_.advertise<sensor_msgs::PointCloud2>(
@ -341,7 +346,6 @@ int Node::AddTrajectory(const TrajectoryOptions& options,
AddExtrapolator(trajectory_id, options); AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options); AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, topics, trajectory_id); LaunchSubscribers(options, topics, trajectory_id);
is_active_trajectory_[trajectory_id] = true;
for (const auto& sensor_id : expected_sensor_ids) { for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id); subscribed_topics_.insert(sensor_id.id);
} }
@ -444,26 +448,26 @@ bool Node::ValidateTopicNames(
cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
const int trajectory_id) { const int trajectory_id) {
auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();
cartographer_ros_msgs::StatusResponse status_response; cartographer_ros_msgs::StatusResponse status_response;
// First, check if we can actually finish the trajectory. // First, check if we can actually finish the trajectory.
if (map_builder_bridge_.GetFrozenTrajectoryIds().count(trajectory_id)) { if (!(trajectory_states.count(trajectory_id))) {
const std::string error =
"Trajectory " + std::to_string(trajectory_id) + " doesn't exist.";
LOG(ERROR) << error;
status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
status_response.message = error;
return status_response;
} else if (trajectory_states.at(trajectory_id) == TrajectoryState::FROZEN) {
const std::string error = const std::string error =
"Trajectory " + std::to_string(trajectory_id) + " is frozen."; "Trajectory " + std::to_string(trajectory_id) + " is frozen.";
LOG(ERROR) << error; LOG(ERROR) << error;
status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
status_response.message = error; status_response.message = error;
return status_response; return status_response;
} } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FINISHED) {
if (is_active_trajectory_.count(trajectory_id) == 0) {
const std::string error =
"Trajectory " + std::to_string(trajectory_id) + " is not created yet.";
LOG(ERROR) << error;
status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
status_response.message = error;
return status_response;
}
if (!is_active_trajectory_[trajectory_id]) {
const std::string error = "Trajectory " + std::to_string(trajectory_id) + const std::string error = "Trajectory " + std::to_string(trajectory_id) +
" has already been finished."; " has already been finished.";
LOG(ERROR) << error; LOG(ERROR) << error;
@ -471,18 +475,27 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
status_response.message = error; status_response.message = error;
return status_response; return status_response;
} else if (trajectory_states.at(trajectory_id) == TrajectoryState::DELETED) {
const std::string error =
"Trajectory " + std::to_string(trajectory_id) + " has been deleted.";
LOG(ERROR) << error;
status_response.code =
cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
status_response.message = error;
return status_response;
} }
// Shutdown the subscribers of this trajectory. // Shutdown the subscribers of this trajectory.
for (auto& entry : subscribers_[trajectory_id]) { // A valid case with no subscribers is e.g. if we just visualize states.
entry.subscriber.shutdown(); if (subscribers_.count(trajectory_id)) {
subscribed_topics_.erase(entry.topic); for (auto& entry : subscribers_[trajectory_id]) {
LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]"; entry.subscriber.shutdown();
subscribed_topics_.erase(entry.topic);
LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
}
CHECK_EQ(subscribers_.erase(trajectory_id), 1);
} }
CHECK_EQ(subscribers_.erase(trajectory_id), 1);
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;
const std::string message = const std::string message =
"Finished trajectory " + std::to_string(trajectory_id) + "."; "Finished trajectory " + std::to_string(trajectory_id) + ".";
status_response.code = cartographer_ros_msgs::StatusCode::OK; status_response.code = cartographer_ros_msgs::StatusCode::OK;
@ -550,10 +563,41 @@ int Node::AddOfflineTrajectory(
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options); AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options); AddSensorSamplers(trajectory_id, options);
is_active_trajectory_[trajectory_id] = true;
return trajectory_id; return trajectory_id;
} }
bool Node::HandleGetTrajectoryStates(
::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
::cartographer_ros_msgs::GetTrajectoryStates::Response& response) {
using TrajectoryState =
::cartographer::mapping::PoseGraphInterface::TrajectoryState;
carto::common::MutexLocker lock(&mutex_);
response.status.code = ::cartographer_ros_msgs::StatusCode::OK;
response.trajectory_states.header.stamp = ros::Time::now();
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
response.trajectory_states.trajectory_id.push_back(entry.first);
switch (entry.second) {
case TrajectoryState::ACTIVE:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::ACTIVE);
break;
case TrajectoryState::FINISHED:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::FINISHED);
break;
case TrajectoryState::FROZEN:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::FROZEN);
break;
case TrajectoryState::DELETED:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::DELETED);
break;
}
}
return true;
}
bool Node::HandleFinishTrajectory( bool Node::HandleFinishTrajectory(
::cartographer_ros_msgs::FinishTrajectory::Request& request, ::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response& response) { ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
@ -578,9 +622,9 @@ bool Node::HandleWriteState(
void Node::FinishAllTrajectories() { void Node::FinishAllTrajectories() {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
for (auto& entry : is_active_trajectory_) { for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
const int trajectory_id = entry.first; if (entry.second == TrajectoryState::ACTIVE) {
if (entry.second) { const int trajectory_id = entry.first;
CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code, CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
cartographer_ros_msgs::StatusCode::OK); cartographer_ros_msgs::StatusCode::OK);
} }
@ -595,9 +639,17 @@ bool Node::FinishTrajectory(const int trajectory_id) {
void Node::RunFinalOptimization() { void Node::RunFinalOptimization() {
{ {
carto::common::MutexLocker lock(&mutex_); for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
for (const auto& entry : is_active_trajectory_) { const int trajectory_id = entry.first;
CHECK(!entry.second); if (entry.second == TrajectoryState::ACTIVE) {
LOG(WARNING)
<< "Can't run final optimization if there are one or more active "
"trajectories. Trying to finish trajectory with ID "
<< std::to_string(trajectory_id) << " now.";
CHECK(FinishTrajectory(trajectory_id))
<< "Failed to finish trajectory with ID "
<< std::to_string(trajectory_id) << ".";
}
} }
} }
// Assuming we are not adding new data anymore, the final optimization // Assuming we are not adding new data anymore, the final optimization

View File

@ -33,6 +33,7 @@
#include "cartographer_ros/node_options.h" #include "cartographer_ros/node_options.h"
#include "cartographer_ros/trajectory_options.h" #include "cartographer_ros/trajectory_options.h"
#include "cartographer_ros_msgs/FinishTrajectory.h" #include "cartographer_ros_msgs/FinishTrajectory.h"
#include "cartographer_ros_msgs/GetTrajectoryStates.h"
#include "cartographer_ros_msgs/SensorTopics.h" #include "cartographer_ros_msgs/SensorTopics.h"
#include "cartographer_ros_msgs/StartTrajectory.h" #include "cartographer_ros_msgs/StartTrajectory.h"
#include "cartographer_ros_msgs/StatusResponse.h" #include "cartographer_ros_msgs/StatusResponse.h"
@ -138,6 +139,10 @@ class Node {
cartographer_ros_msgs::FinishTrajectory::Response& response); cartographer_ros_msgs::FinishTrajectory::Response& response);
bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request, bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request,
cartographer_ros_msgs::WriteState::Response& response); cartographer_ros_msgs::WriteState::Response& response);
bool HandleGetTrajectoryStates(
::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
::cartographer_ros_msgs::GetTrajectoryStates::Response& response);
// Returns the set of SensorIds expected for a trajectory. // Returns the set of SensorIds expected for a trajectory.
// 'SensorId::id' is the expected ROS topic name. // 'SensorId::id' is the expected ROS topic name.
std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId> std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>
@ -203,7 +208,6 @@ class Node {
std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_; std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
std::unordered_map<int, std::vector<Subscriber>> subscribers_; std::unordered_map<int, std::vector<Subscriber>> subscribers_;
std::unordered_set<std::string> subscribed_topics_; std::unordered_set<std::string> subscribed_topics_;
std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_);
// We have to keep the timer handles of ::ros::WallTimers around, otherwise // We have to keep the timer handles of ::ros::WallTimers around, otherwise
// they do not fire. // they do not fire.

View File

@ -37,6 +37,7 @@ constexpr char kSubmapListTopic[] = "submap_list";
constexpr char kSubmapQueryServiceName[] = "submap_query"; constexpr char kSubmapQueryServiceName[] = "submap_query";
constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
constexpr char kWriteStateServiceName[] = "write_state"; constexpr char kWriteStateServiceName[] = "write_state";
constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states";
constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list"; constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list"; constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";
constexpr char kConstraintListTopic[] = "constraint_list"; constexpr char kConstraintListTopic[] = "constraint_list";

View File

@ -34,13 +34,15 @@ add_message_files(
SubmapTexture.msg SubmapTexture.msg
SensorTopics.msg SensorTopics.msg
TrajectoryOptions.msg TrajectoryOptions.msg
TrajectoryStates.msg
) )
add_service_files( add_service_files(
FILES FILES
SubmapQuery.srv
FinishTrajectory.srv FinishTrajectory.srv
GetTrajectoryStates.srv
StartTrajectory.srv StartTrajectory.srv
SubmapQuery.srv
WriteState.srv WriteState.srv
) )

View File

@ -0,0 +1,22 @@
# Copyright 2018 The Cartographer Authors
#
# Licensed under the Apache License, Version 2.0 (the 'License');
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an 'AS IS' BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
uint8 ACTIVE = 0
uint8 FINISHED = 1
uint8 FROZEN = 2
uint8 DELETED = 3
std_msgs/Header header
int32[] trajectory_id
uint8[] trajectory_state

View File

@ -0,0 +1,18 @@
# Copyright 2018 The Cartographer Authors
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
---
cartographer_ros_msgs/StatusResponse status
cartographer_ros_msgs/TrajectoryStates trajectory_states