Service to query trajectory segments from the pose graph. (#1222)

This `/trajectory_query` service allows to look up trajectory segments
from the pose graph.

This can be useful if one wants to get optimized trajectory data from
the current SLAM run and listening to live TF data would be too noisy.
For example, to stitch buffered point cloud data from a depth sensor
based on a recent localization trajectory segment of a robot.

Before, the pose graph trajectory nodes were not available except for
the trajectory marker topic (only positions, no orientation; inefficient)
or after serialization (which is not practical in live operation).

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -22,6 +22,7 @@
 #include "cartographer/io/proto_stream.h"
 #include "cartographer/mapping/pose_graph.h"
 #include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros/time_conversion.h"
 #include "cartographer_ros_msgs/StatusCode.h"
 #include "cartographer_ros_msgs/StatusResponse.h"
 
@@ -258,6 +259,30 @@ MapBuilderBridge::GetLocalTrajectoryData() {
   return local_trajectory_data;
 }
 
+void MapBuilderBridge::HandleTrajectoryQuery(
+    cartographer_ros_msgs::TrajectoryQuery::Request& request,
+    cartographer_ros_msgs::TrajectoryQuery::Response& response) {
+  // This query is safe if the trajectory doesn't exist (returns 0 poses).
+  // However, we can filter unwanted states at the higher level in the node.
+  const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();
+  for (const auto& node_id_data :
+       node_poses.trajectory(request.trajectory_id)) {
+    if (!node_id_data.data.constant_pose_data.has_value()) {
+      continue;
+    }
+    geometry_msgs::PoseStamped pose_stamped;
+    pose_stamped.header.frame_id = node_options_.map_frame;
+    pose_stamped.header.stamp =
+        ToRos(node_id_data.data.constant_pose_data.value().time);
+    pose_stamped.pose = ToGeometryMsgPose(node_id_data.data.global_pose);
+    response.trajectory.push_back(pose_stamped);
+  }
+  response.status.code = cartographer_ros_msgs::StatusCode::OK;
+  response.status.message = absl::StrCat(
+      "Retrieved ", response.trajectory.size(),
+      " trajectory nodes from trajectory ", request.trajectory_id, ".");
+}
+
 visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
   visualization_msgs::MarkerArray trajectory_node_list;
   const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -34,6 +34,8 @@
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapList.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
+#include "cartographer_ros_msgs/TrajectoryQuery.h"
+#include "geometry_msgs/TransformStamped.h"
 #include "nav_msgs/OccupancyGrid.h"
 
 // Abseil unfortunately pulls in winnt.h, which #defines DELETE.
@@ -84,6 +86,9 @@ class MapBuilderBridge {
   void HandleSubmapQuery(
       cartographer_ros_msgs::SubmapQuery::Request& request,
       cartographer_ros_msgs::SubmapQuery::Response& response);
+  void HandleTrajectoryQuery(
+      cartographer_ros_msgs::TrajectoryQuery::Request& request,
+      cartographer_ros_msgs::TrajectoryQuery::Response& response);
 
   std::map<int /* trajectory_id */,
            ::cartographer::mapping::PoseGraphInterface::TrajectoryState>

cartographer_ros/cartographer_ros/node.cc

@@ -128,6 +128,8 @@ Node::Node(
           kConstraintListTopic, kLatestOnlyPublisherQueueSize);
   service_servers_.push_back(node_handle_.advertiseService(
       kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
+  service_servers_.push_back(node_handle_.advertiseService(
+      kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
   service_servers_.push_back(node_handle_.advertiseService(
       kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
   service_servers_.push_back(node_handle_.advertiseService(
@@ -174,6 +176,23 @@ bool Node::HandleSubmapQuery(
   return true;
 }
 
+bool Node::HandleTrajectoryQuery(
+    ::cartographer_ros_msgs::TrajectoryQuery::Request& request,
+    ::cartographer_ros_msgs::TrajectoryQuery::Response& response) {
+  absl::MutexLock lock(&mutex_);
+  response.status = TrajectoryStateToStatus(
+      request.trajectory_id,
+      {TrajectoryState::ACTIVE, TrajectoryState::FINISHED,
+       TrajectoryState::FROZEN} /* valid states */);
+  if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
+    LOG(ERROR) << "Can't query trajectory from pose graph: "
+               << response.status.message;
+    return true;
+  }
+  map_builder_bridge_.HandleTrajectoryQuery(request, response);
+  return true;
+}
+
 void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
   absl::MutexLock lock(&mutex_);
   submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
@@ -485,22 +504,21 @@ cartographer_ros_msgs::StatusResponse Node::TrajectoryStateToStatus(
   const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();
   cartographer_ros_msgs::StatusResponse status_response;
 
-  if (!(trajectory_states.count(trajectory_id))) {
+  const auto it = trajectory_states.find(trajectory_id);
+  if (it == trajectory_states.end()) {
     status_response.message =
         absl::StrCat("Trajectory ", trajectory_id, " doesn't exist.");
     status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
     return status_response;
   }
 
-  const auto trajectory_state = trajectory_states.at(trajectory_id);
   status_response.message =
       absl::StrCat("Trajectory ", trajectory_id, " is in '",
-                   TrajectoryStateToString(trajectory_state), "' state.");
+                   TrajectoryStateToString(it->second), "' state.");
-  if (valid_states.count(trajectory_state)) {
+  status_response.code =
-    status_response.code = cartographer_ros_msgs::StatusCode::OK;
+      valid_states.count(it->second)
-  } else {
+          ? cartographer_ros_msgs::StatusCode::OK
-    status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
+          : cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
-  }
   return status_response;
 }
 

cartographer_ros/cartographer_ros/node.h

@@ -134,6 +134,9 @@ class Node {
   bool HandleSubmapQuery(
       cartographer_ros_msgs::SubmapQuery::Request& request,
       cartographer_ros_msgs::SubmapQuery::Response& response);
+  bool HandleTrajectoryQuery(
+      ::cartographer_ros_msgs::TrajectoryQuery::Request& request,
+      ::cartographer_ros_msgs::TrajectoryQuery::Response& response);
   bool HandleStartTrajectory(
       cartographer_ros_msgs::StartTrajectory::Request& request,
       cartographer_ros_msgs::StartTrajectory::Response& response);

cartographer_ros/cartographer_ros/node_constants.h

@@ -35,6 +35,7 @@ constexpr char kOccupancyGridTopic[] = "map";
 constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
 constexpr char kSubmapListTopic[] = "submap_list";
 constexpr char kSubmapQueryServiceName[] = "submap_query";
+constexpr char kTrajectoryQueryServiceName[] = "trajectory_query";
 constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
 constexpr char kWriteStateServiceName[] = "write_state";
 constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states";

cartographer_ros_msgs/CMakeLists.txt

@@ -51,6 +51,7 @@ add_service_files(
     ReadMetrics.srv
     StartTrajectory.srv
     SubmapQuery.srv
+    TrajectoryQuery.srv
     WriteState.srv
 )
 

cartographer_ros_msgs/srv/TrajectoryQuery.srv

@@ -0,0 +1,18 @@
+# Copyright 2019 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.
+
+int32 trajectory_id
+---
+cartographer_ros_msgs/StatusResponse status
+geometry_msgs/PoseStamped[] trajectory