From c57641f917d71204f8ed941bb4ed4dc7e6227c75 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= <juraj.orsulic@fer.hr>
Date: Thu, 2 Nov 2017 10:28:13 +0100
Subject: [PATCH] Make SPG::AddScan return the inserted node id. (#617)

---
 cartographer/mapping_2d/sparse_pose_graph.cc | 3 ++-
 cartographer/mapping_2d/sparse_pose_graph.h  | 2 +-
 cartographer/mapping_3d/sparse_pose_graph.cc | 3 ++-
 cartographer/mapping_3d/sparse_pose_graph.h  | 2 +-
 4 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc
index b1279b1..ad9c0e2 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.cc
+++ b/cartographer/mapping_2d/sparse_pose_graph.cc
@@ -93,7 +93,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
   return {front_submap_id, last_submap_id};
 }
 
-void SparsePoseGraph::AddScan(
+mapping::NodeId SparsePoseGraph::AddScan(
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
     const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
@@ -124,6 +124,7 @@ void SparsePoseGraph::AddScan(
     ComputeConstraintsForScan(node_id, insertion_submaps,
                               newly_finished_submap);
   });
+  return node_id;
 }
 
 void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h
index b0b60fb..f370380 100644
--- a/cartographer/mapping_2d/sparse_pose_graph.h
+++ b/cartographer/mapping_2d/sparse_pose_graph.h
@@ -70,7 +70,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // 'insertion_submaps.front()' and the scan was inserted into the
   // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
   // 'true', this submap was inserted into for the last time.
-  void AddScan(
+  mapping::NodeId AddScan(
       std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
       int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc
index 86e00c3..ed79a71 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.cc
+++ b/cartographer/mapping_3d/sparse_pose_graph.cc
@@ -90,7 +90,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
   return {front_submap_id, last_submap_id};
 }
 
-void SparsePoseGraph::AddScan(
+mapping::NodeId SparsePoseGraph::AddScan(
     std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
     const int trajectory_id,
     const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
@@ -121,6 +121,7 @@ void SparsePoseGraph::AddScan(
     ComputeConstraintsForScan(node_id, insertion_submaps,
                               newly_finished_submap);
   });
+  return node_id;
 }
 
 void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h
index 1b24980..fddd023 100644
--- a/cartographer/mapping_3d/sparse_pose_graph.h
+++ b/cartographer/mapping_3d/sparse_pose_graph.h
@@ -70,7 +70,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // 'insertion_submaps.front()' and the scan was inserted into the
   // 'insertion_submaps'. If 'insertion_submaps.front().finished()' is
   // 'true', this submap was inserted into for the last time.
-  void AddScan(
+  mapping::NodeId AddScan(
       std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data,
       int trajectory_id,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)