From f5b583fde8d857f72c45da3820389b4227d3b084 Mon Sep 17 00:00:00 2001
From: Michael Grupp <grupp@magazino.eu>
Date: Fri, 29 Jun 2018 13:28:40 +0200
Subject: [PATCH] Allow to ignore (un-)frozen submaps in the occupancy grid
 node. (#899)

---
 .../cartographer_ros/map_builder_bridge.cc        |  2 ++
 .../cartographer_ros/occupancy_grid_node_main.cc  | 15 ++++++++++++++-
 cartographer_ros_msgs/msg/SubmapEntry.msg         |  1 +
 3 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
index ba2c6f2..227cc9c 100644
--- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc
+++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc
@@ -206,6 +206,8 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
   for (const auto& submap_id_pose :
        map_builder_->pose_graph()->GetAllSubmapPoses()) {
     cartographer_ros_msgs::SubmapEntry submap_entry;
+    submap_entry.is_frozen = map_builder_->pose_graph()->IsTrajectoryFrozen(
+        submap_id_pose.id.trajectory_id);
     submap_entry.trajectory_id = submap_id_pose.id.trajectory_id;
     submap_entry.submap_index = submap_id_pose.id.submap_index;
     submap_entry.submap_version = submap_id_pose.data.version;
diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
index 8385fad..a662f11 100644
--- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
+++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
@@ -40,6 +40,12 @@
 DEFINE_double(resolution, 0.05,
               "Resolution of a grid cell in the published occupancy grid.");
 DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period.");
+DEFINE_bool(include_frozen_submaps, true,
+            "Include frozen submaps in the occupancy grid.");
+DEFINE_bool(include_unfrozen_submaps, true,
+            "Include unfrozen submaps in the occupancy grid.");
+DEFINE_string(occupancy_grid_topic, cartographer_ros::kOccupancyGridTopic,
+              "Name of the topic on which the occupancy grid is published.");
 
 namespace cartographer_ros {
 namespace {
@@ -89,7 +95,7 @@ Node::Node(const double resolution, const double publish_period_sec)
               }))),
       occupancy_grid_publisher_(
           node_handle_.advertise<::nav_msgs::OccupancyGrid>(
-              kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,
+              FLAGS_occupancy_grid_topic, kLatestOnlyPublisherQueueSize,
               true /* latched */)),
       occupancy_grid_publisher_timer_(
           node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec),
@@ -113,6 +119,10 @@ void Node::HandleSubmapList(
   for (const auto& submap_msg : msg->submap) {
     const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index};
     submap_ids_to_delete.erase(id);
+    if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) ||
+        (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) {
+      continue;
+    }
     SubmapSlice& submap_slice = submap_slices_[id];
     submap_slice.pose = ToRigid3d(submap_msg.pose);
     submap_slice.metadata_version = submap_msg.submap_version;
@@ -172,6 +182,9 @@ int main(int argc, char** argv) {
   google::InitGoogleLogging(argv[0]);
   google::ParseCommandLineFlags(&argc, &argv, true);
 
+  CHECK(FLAGS_include_frozen_submaps || FLAGS_include_unfrozen_submaps)
+      << "Ignoring both frozen and unfrozen submaps makes no sense.";
+
   ::ros::init(argc, argv, "cartographer_occupancy_grid_node");
   ::ros::start();
 
diff --git a/cartographer_ros_msgs/msg/SubmapEntry.msg b/cartographer_ros_msgs/msg/SubmapEntry.msg
index 4ea51f9..2a38f7c 100644
--- a/cartographer_ros_msgs/msg/SubmapEntry.msg
+++ b/cartographer_ros_msgs/msg/SubmapEntry.msg
@@ -16,3 +16,4 @@ int32 trajectory_id
 int32 submap_index
 int32 submap_version
 geometry_msgs/Pose pose
+bool is_frozen