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/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<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;
+std::map<int, ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
+MapBuilderBridge::GetTrajectoryStates() {
+  return map_builder_->pose_graph()->GetTrajectoryStates();
 }
 
 cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {

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<int> GetFrozenTrajectoryIds();
+  std::map<int /* trajectory_id */,
+           ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
+  GetTrajectoryStates();
   cartographer_ros_msgs::SubmapList GetSubmapList();
   std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
       EXCLUDES(mutex_);

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 <typename MessageType>
 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<sensor_msgs::PointCloud2>(
@@ -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

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<int, TrajectorySensorSamplers> sensor_samplers_;
   std::unordered_map<int, std::vector<Subscriber>> subscribers_;
   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
   // they do not fire.

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";

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(

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

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