From 8285673abb1a81760827a26e2f9052fc23d0a1d8 Mon Sep 17 00:00:00 2001
From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com>
Date: Tue, 2 Oct 2018 17:07:04 +0200
Subject: [PATCH] [GenericPoseGraph] Add 'group_id' field to NodeId. (#1438)

---
 cartographer/pose_graph/node/node_id.cc      |  8 +++++++-
 cartographer/pose_graph/node/node_id.h       | 15 ++++++++++++---
 cartographer/pose_graph/node/node_id_test.cc | 16 +++++++++++++---
 cartographer/pose_graph/proto/node.proto     |  3 ++-
 4 files changed, 34 insertions(+), 8 deletions(-)

diff --git a/cartographer/pose_graph/node/node_id.cc b/cartographer/pose_graph/node/node_id.cc
index c340feb..b242b33 100644
--- a/cartographer/pose_graph/node/node_id.cc
+++ b/cartographer/pose_graph/node/node_id.cc
@@ -22,12 +22,18 @@ namespace pose_graph {
 NodeId::NodeId(const std::string& object_id, common::Time time)
     : object_id(object_id), time(time) {}
 
+NodeId::NodeId(const std::string& object_id, const std::string& group_id,
+               common::Time time)
+    : object_id(object_id), group_id(group_id), time(time) {}
+
 NodeId::NodeId(const proto::NodeId& node_id)
-    : NodeId(node_id.object_id(), common::FromUniversal(node_id.timestamp())) {}
+    : NodeId(node_id.object_id(), node_id.group_id(),
+             common::FromUniversal(node_id.timestamp())) {}
 
 proto::NodeId NodeId::ToProto() const {
   proto::NodeId node_id;
   node_id.set_object_id(object_id);
+  node_id.set_group_id(group_id);
   node_id.set_timestamp(common::ToUniversal(time));
   return node_id;
 }
diff --git a/cartographer/pose_graph/node/node_id.h b/cartographer/pose_graph/node/node_id.h
index 7130211..ecb62b1 100644
--- a/cartographer/pose_graph/node/node_id.h
+++ b/cartographer/pose_graph/node/node_id.h
@@ -28,11 +28,15 @@ namespace pose_graph {
 
 struct NodeId {
   NodeId(const std::string& object_id, common::Time time);
+  NodeId(const std::string& object_id, const std::string& group_id,
+         common::Time time);
   explicit NodeId(const proto::NodeId& node_id);
 
   // Object refers to dynamic/static objects, e.g. robot/landmark/submap poses,
   // IMU calibration, etc.
   std::string object_id;
+  // Id of the group to which the node belongs, e.g. "submap".
+  std::string group_id;
   // Time associated with the object's pose.
   common::Time time;
 
@@ -40,12 +44,17 @@ struct NodeId {
 };
 
 inline bool operator<(const NodeId& lhs, const NodeId& rhs) {
-  return std::forward_as_tuple(lhs.object_id, lhs.time) <
-         std::forward_as_tuple(rhs.object_id, rhs.time);
+  return std::forward_as_tuple(lhs.object_id, lhs.group_id, lhs.time) <
+         std::forward_as_tuple(rhs.object_id, rhs.group_id, rhs.time);
 }
 
 inline std::ostream& operator<<(std::ostream& os, const NodeId& id) {
-  return os << "(object_id: " << id.object_id << ", time: " << id.time << ")";
+  std::string group_message;
+  if (!id.group_id.empty()) {
+    group_message = ", group_id: " + id.group_id;
+  }
+  return os << "(object_id: " << id.object_id << group_message
+            << ", time: " << id.time << ")";
 }
 
 }  // namespace pose_graph
diff --git a/cartographer/pose_graph/node/node_id_test.cc b/cartographer/pose_graph/node/node_id_test.cc
index 53d9d83..37a5a18 100644
--- a/cartographer/pose_graph/node/node_id_test.cc
+++ b/cartographer/pose_graph/node/node_id_test.cc
@@ -27,26 +27,36 @@ namespace {
 TEST(NodeIdTest, FromProto) {
   proto::NodeId proto;
   proto.set_object_id("some_object");
+  proto.set_group_id("submap");
   proto.set_timestamp(1);
 
   NodeId node_id(proto);
   EXPECT_EQ(node_id.object_id, "some_object");
+  EXPECT_EQ(node_id.group_id, "submap");
   EXPECT_EQ(common::ToUniversal(node_id.time), 1);
 }
 
 TEST(NodeIdTest, ToProto) {
-  NodeId node_id{"some_object", common::FromUniversal(1)};
+  NodeId node_id{"some_object", "submap", common::FromUniversal(1)};
   EXPECT_THAT(node_id.ToProto(),
-              testing::EqualsProto("object_id: 'some_object' timestamp: 1"));
+              testing::EqualsProto(
+                  "object_id: 'some_object' group_id: 'submap' timestamp: 1"));
 }
 
-TEST(NodeIdTest, ToString) {
+TEST(NodeIdTest, ToStringWithoutGroupId) {
   std::stringstream ss;
   ss << NodeId{"some_object", common::FromUniversal(1)};
 
   EXPECT_EQ("(object_id: some_object, time: 1)", ss.str());
 }
 
+TEST(NodeIdTest, ToStringWithGroupId) {
+  std::stringstream ss;
+  ss << NodeId{"some_object", "submap", common::FromUniversal(1)};
+
+  EXPECT_EQ("(object_id: some_object, group_id: submap, time: 1)", ss.str());
+}
+
 }  // namespace
 }  // namespace pose_graph
 }  // namespace cartographer
diff --git a/cartographer/pose_graph/proto/node.proto b/cartographer/pose_graph/proto/node.proto
index 8b73b29..597a95e 100644
--- a/cartographer/pose_graph/proto/node.proto
+++ b/cartographer/pose_graph/proto/node.proto
@@ -45,7 +45,8 @@ message ImuCalibration {
 
 message NodeId {
   string object_id = 1;
-  int64 timestamp = 2;
+  string group_id = 2;
+  int64 timestamp = 3;
 }
 
 message Parameters {