From 8bbec6bff77a7e63669db34b9023ad1811e741fd Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Fri, 29 Jun 2018 11:14:56 +0200 Subject: [PATCH] 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) --- .../cartographer_ros/map_builder_bridge.cc | 14 +-- .../cartographer_ros/map_builder_bridge.h | 5 +- cartographer_ros/cartographer_ros/node.cc | 104 +++++++++++++----- cartographer_ros/cartographer_ros/node.h | 6 +- .../cartographer_ros/node_constants.h | 1 + cartographer_ros_msgs/CMakeLists.txt | 6 +- .../msg/TrajectoryStates.msg | 22 ++++ .../srv/GetTrajectoryStates.srv | 18 +++ 8 files changed, 136 insertions(+), 40 deletions(-) create mode 100644 cartographer_ros_msgs/msg/TrajectoryStates.msg create mode 100644 cartographer_ros_msgs/srv/GetTrajectoryStates.srv diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index ad57927..ba2c6f2 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -147,7 +147,7 @@ void MapBuilderBridge::FinishTrajectory(const int trajectory_id) { LOG(INFO) << "Finishing trajectory with ID '" << 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); sensor_bridges_.erase(trajectory_id); } @@ -194,15 +194,9 @@ void MapBuilderBridge::HandleSubmapQuery( response.status.code = cartographer_ros_msgs::StatusCode::OK; } -std::set MapBuilderBridge::GetFrozenTrajectoryIds() { - std::set 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; +std::map +MapBuilderBridge::GetTrajectoryStates() { + return map_builder_->pose_graph()->GetTrajectoryStates(); } cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 2daaa45..d79c071 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -24,6 +24,7 @@ #include "cartographer/common/mutex.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/trajectory_builder_interface.h" #include "cartographer_ros/node_options.h" @@ -77,7 +78,9 @@ class MapBuilderBridge { cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Response& response); - std::set GetFrozenTrajectoryIds(); + std::map + GetTrajectoryStates(); cartographer_ros_msgs::SubmapList GetSubmapList(); std::unordered_map GetLocalTrajectoryData() EXCLUDES(mutex_); diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 25ce75c..8cc4852 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -26,6 +26,7 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" +#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" @@ -81,6 +82,8 @@ template namespace carto = ::cartographer; using carto::transform::Rigid3d; +using TrajectoryState = + ::cartographer::mapping::PoseGraphInterface::TrajectoryState; Node::Node( const NodeOptions& node_options, @@ -109,6 +112,8 @@ Node::Node( kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this)); service_servers_.push_back(node_handle_.advertiseService( kWriteStateServiceName, &Node::HandleWriteState, this)); + service_servers_.push_back(node_handle_.advertiseService( + kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this)); scan_matched_point_cloud_publisher_ = node_handle_.advertise( @@ -341,7 +346,6 @@ int Node::AddTrajectory(const TrajectoryOptions& options, AddExtrapolator(trajectory_id, options); AddSensorSamplers(trajectory_id, options); LaunchSubscribers(options, topics, trajectory_id); - is_active_trajectory_[trajectory_id] = true; for (const auto& sensor_id : expected_sensor_ids) { subscribed_topics_.insert(sensor_id.id); } @@ -444,26 +448,26 @@ bool Node::ValidateTopicNames( cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( const int trajectory_id) { + auto trajectory_states = map_builder_bridge_.GetTrajectoryStates(); + cartographer_ros_msgs::StatusResponse status_response; // 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 = "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(ERROR) << error; - status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; - status_response.message = error; - return status_response; - } - if (!is_active_trajectory_[trajectory_id]) { + } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FINISHED) { const std::string error = "Trajectory " + std::to_string(trajectory_id) + " has already been finished."; LOG(ERROR) << error; @@ -471,18 +475,27 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; status_response.message = error; 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. - for (auto& entry : subscribers_[trajectory_id]) { - entry.subscriber.shutdown(); - subscribed_topics_.erase(entry.topic); - LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]"; + // A valid case with no subscribers is e.g. if we just visualize states. + if (subscribers_.count(trajectory_id)) { + 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_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; const std::string message = "Finished trajectory " + std::to_string(trajectory_id) + "."; status_response.code = cartographer_ros_msgs::StatusCode::OK; @@ -550,10 +563,41 @@ int Node::AddOfflineTrajectory( map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); AddSensorSamplers(trajectory_id, options); - is_active_trajectory_[trajectory_id] = true; 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( ::cartographer_ros_msgs::FinishTrajectory::Request& request, ::cartographer_ros_msgs::FinishTrajectory::Response& response) { @@ -578,9 +622,9 @@ bool Node::HandleWriteState( void Node::FinishAllTrajectories() { carto::common::MutexLocker lock(&mutex_); - for (auto& entry : is_active_trajectory_) { - const int trajectory_id = entry.first; - if (entry.second) { + for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { + if (entry.second == TrajectoryState::ACTIVE) { + const int trajectory_id = entry.first; CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code, cartographer_ros_msgs::StatusCode::OK); } @@ -595,9 +639,17 @@ bool Node::FinishTrajectory(const int trajectory_id) { void Node::RunFinalOptimization() { { - carto::common::MutexLocker lock(&mutex_); - for (const auto& entry : is_active_trajectory_) { - CHECK(!entry.second); + for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { + const int trajectory_id = entry.first; + 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 diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 9e855a8..1ea0608 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -33,6 +33,7 @@ #include "cartographer_ros/node_options.h" #include "cartographer_ros/trajectory_options.h" #include "cartographer_ros_msgs/FinishTrajectory.h" +#include "cartographer_ros_msgs/GetTrajectoryStates.h" #include "cartographer_ros_msgs/SensorTopics.h" #include "cartographer_ros_msgs/StartTrajectory.h" #include "cartographer_ros_msgs/StatusResponse.h" @@ -138,6 +139,10 @@ class Node { cartographer_ros_msgs::FinishTrajectory::Response& response); bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request, 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. // 'SensorId::id' is the expected ROS topic name. std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId> @@ -203,7 +208,6 @@ class Node { std::unordered_map sensor_samplers_; std::unordered_map> subscribers_; std::unordered_set subscribed_topics_; - std::unordered_map is_active_trajectory_ GUARDED_BY(mutex_); // We have to keep the timer handles of ::ros::WallTimers around, otherwise // they do not fire. diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index 1065519..b658d16 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/cartographer_ros/node_constants.h @@ -37,6 +37,7 @@ constexpr char kSubmapListTopic[] = "submap_list"; constexpr char kSubmapQueryServiceName[] = "submap_query"; constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; constexpr char kWriteStateServiceName[] = "write_state"; +constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states"; constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list"; constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list"; constexpr char kConstraintListTopic[] = "constraint_list"; diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index 4c6b31d..3481e66 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -34,14 +34,16 @@ add_message_files( SubmapTexture.msg SensorTopics.msg TrajectoryOptions.msg + TrajectoryStates.msg ) add_service_files( FILES - SubmapQuery.srv FinishTrajectory.srv + GetTrajectoryStates.srv StartTrajectory.srv - WriteState.srv + SubmapQuery.srv + WriteState.srv ) generate_messages( diff --git a/cartographer_ros_msgs/msg/TrajectoryStates.msg b/cartographer_ros_msgs/msg/TrajectoryStates.msg new file mode 100644 index 0000000..8f3a43b --- /dev/null +++ b/cartographer_ros_msgs/msg/TrajectoryStates.msg @@ -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 diff --git a/cartographer_ros_msgs/srv/GetTrajectoryStates.srv b/cartographer_ros_msgs/srv/GetTrajectoryStates.srv new file mode 100644 index 0000000..5ae7a90 --- /dev/null +++ b/cartographer_ros_msgs/srv/GetTrajectoryStates.srv @@ -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