Provide local poses via GetTrajectoryNodePoses() (#1172)

cartographer/cloud/internal/client/pose_graph_stub.cc

@@ -89,11 +89,18 @@ PoseGraphStub::GetTrajectoryNodePoses() const {
   CHECK(client.Write(request));
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> node_poses;
   for (const auto& node_pose : client.response().node_poses()) {
-    node_poses.Insert(mapping::NodeId{node_pose.node_id().trajectory_id(),
+    common::optional<mapping::TrajectoryNodePose::ConstantPoseData>
+        constant_pose_data;
+    if (node_pose.has_constant_pose_data()) {
+      constant_pose_data = mapping::TrajectoryNodePose::ConstantPoseData{
+          common::FromUniversal(node_pose.constant_pose_data().timestamp()),
+          transform::ToRigid3(node_pose.constant_pose_data().local_pose())};
+    }
+    node_poses.Insert(
+        mapping::NodeId{node_pose.node_id().trajectory_id(),
                         node_pose.node_id().node_index()},
         mapping::TrajectoryNodePose{
-                          node_pose.has_constant_data(),
+            transform::ToRigid3(node_pose.global_pose()), constant_pose_data});
-                          transform::ToRigid3(node_pose.global_pose())});
   }
   return node_poses;
 }

cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.cc

@@ -39,7 +39,14 @@ void GetTrajectoryNodePosesHandler::OnRequest(
     node_id_pose.id.ToProto(node_pose->mutable_node_id());
     *node_pose->mutable_global_pose() =
         transform::ToProto(node_id_pose.data.global_pose);
-    node_pose->set_has_constant_data(node_id_pose.data.has_constant_data);
+    if (node_id_pose.data.constant_pose_data.has_value()) {
+      node_pose->mutable_constant_pose_data()->set_timestamp(
+          common::ToUniversal(
+              node_id_pose.data.constant_pose_data.value().time));
+      *node_pose->mutable_constant_pose_data()->mutable_local_pose() =
+          transform::ToProto(
+              node_id_pose.data.constant_pose_data.value().local_pose);
+    }
   }
   Send(std::move(response));
 }

cartographer/cloud/proto/map_builder_service.proto

@@ -140,9 +140,13 @@ message GetSubmapResponse {
 }
 
 message TrajectoryNodePose {
+  message ConstantPoseData {
+    int64 timestamp = 1;
+    cartographer.transform.proto.Rigid3d local_pose = 2;
+  }
   cartographer.mapping.proto.NodeId node_id = 1;
   cartographer.transform.proto.Rigid3d global_pose = 2;
-  bool has_constant_data = 3;
+  ConstantPoseData constant_pose_data = 3;
 }
 
 message GetTrajectoryNodePosesResponse {

cartographer/mapping/internal/2d/pose_graph_2d.cc

@@ -630,10 +630,15 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses()
   MapById<NodeId, TrajectoryNodePose> node_poses;
   common::MutexLocker locker(&mutex_);
   for (const auto& node_id_data : trajectory_nodes_) {
+    common::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
+    if (node_id_data.data.constant_data != nullptr) {
+      constant_pose_data = TrajectoryNodePose::ConstantPoseData{
+          node_id_data.data.constant_data->time,
+          node_id_data.data.constant_data->local_pose};
+    }
     node_poses.Insert(
         node_id_data.id,
-        TrajectoryNodePose{node_id_data.data.constant_data != nullptr,
+        TrajectoryNodePose{node_id_data.data.global_pose, constant_pose_data});
-                           node_id_data.data.global_pose});
   }
   return node_poses;
 }

cartographer/mapping/internal/3d/pose_graph_3d.cc

@@ -671,10 +671,15 @@ MapById<NodeId, TrajectoryNodePose> PoseGraph3D::GetTrajectoryNodePoses()
   MapById<NodeId, TrajectoryNodePose> node_poses;
   common::MutexLocker locker(&mutex_);
   for (const auto& node_id_data : trajectory_nodes_) {
+    common::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
+    if (node_id_data.data.constant_data != nullptr) {
+      constant_pose_data = TrajectoryNodePose::ConstantPoseData{
+          node_id_data.data.constant_data->time,
+          node_id_data.data.constant_data->local_pose};
+    }
     node_poses.Insert(
         node_id_data.id,
-        TrajectoryNodePose{node_id_data.data.constant_data != nullptr,
+        TrajectoryNodePose{node_id_data.data.global_pose, constant_pose_data});
-                           node_id_data.data.global_pose});
   }
   return node_poses;
 }

cartographer/mapping/trajectory_node.h

@@ -21,6 +21,7 @@
 #include <vector>
 
 #include "Eigen/Core"
+#include "cartographer/common/optional.h"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/proto/trajectory_node_data.pb.h"
 #include "cartographer/sensor/range_data.h"
@@ -30,11 +31,14 @@ namespace cartographer {
 namespace mapping {
 
 struct TrajectoryNodePose {
-  // Indicates whether the node has constant data set.
+  struct ConstantPoseData {
-  bool has_constant_data;
+    common::Time time;
-
+    transform::Rigid3d local_pose;
+  };
   // The node pose in the global SLAM frame.
   transform::Rigid3d global_pose;
+
+  common::optional<ConstantPoseData> constant_pose_data;
 };
 
 struct TrajectoryNode {