diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index 33e15f7..67199d4 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -89,11 +89,18 @@ PoseGraphStub::GetTrajectoryNodePoses() const { CHECK(client.Write(request)); mapping::MapById node_poses; for (const auto& node_pose : client.response().node_poses()) { - 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())}); + common::optional + 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{ + transform::ToRigid3(node_pose.global_pose()), constant_pose_data}); } return node_poses; } diff --git a/cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.cc b/cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.cc index ddc879e..8880fec 100644 --- a/cartographer/cloud/internal/handlers/get_trajectory_node_poses_handler.cc +++ b/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)); } diff --git a/cartographer/cloud/proto/map_builder_service.proto b/cartographer/cloud/proto/map_builder_service.proto index b3e88d7..51fef3b 100644 --- a/cartographer/cloud/proto/map_builder_service.proto +++ b/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 { diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index be02136..49d0c0c 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -630,10 +630,15 @@ MapById PoseGraph2D::GetTrajectoryNodePoses() MapById node_poses; common::MutexLocker locker(&mutex_); for (const auto& node_id_data : trajectory_nodes_) { + common::optional 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, - node_id_data.data.global_pose}); + TrajectoryNodePose{node_id_data.data.global_pose, constant_pose_data}); } return node_poses; } diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index ed7e6b0..a2db4e7 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -671,10 +671,15 @@ MapById PoseGraph3D::GetTrajectoryNodePoses() MapById node_poses; common::MutexLocker locker(&mutex_); for (const auto& node_id_data : trajectory_nodes_) { + common::optional 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, - node_id_data.data.global_pose}); + TrajectoryNodePose{node_id_data.data.global_pose, constant_pose_data}); } return node_poses; } diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index e0ee709..12c2057 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -21,6 +21,7 @@ #include #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. - bool has_constant_data; - + struct ConstantPoseData { + common::Time time; + transform::Rigid3d local_pose; + }; // The node pose in the global SLAM frame. transform::Rigid3d global_pose; + + common::optional constant_pose_data; }; struct TrajectoryNode {