Handle trimmed submaps in the visualization. (#389)

Related to googlecartographer/cartographer#283.

PAIR=SirVer

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -20,7 +20,6 @@
 #include "cartographer_ros/color.h"
 #include "cartographer_ros/msg_conversion.h"
 #include "cartographer_ros/occupancy_grid.h"
-#include "cartographer_ros_msgs/TrajectorySubmapList.h"
 
 namespace cartographer_ros {
 
@@ -133,14 +132,19 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
       map_builder_.sparse_pose_graph()->GetAllSubmapData();
   for (size_t trajectory_id = 0; trajectory_id < all_submap_data.size();
        ++trajectory_id) {
-    cartographer_ros_msgs::TrajectorySubmapList trajectory_submap_list;
-    for (const auto& submap_data : all_submap_data[trajectory_id]) {
+    for (size_t submap_index = 0;
+         submap_index < all_submap_data[trajectory_id].size(); ++submap_index) {
+      const auto& submap_data = all_submap_data[trajectory_id][submap_index];
+      if (submap_data.submap == nullptr) {
+        continue;
+      }
       cartographer_ros_msgs::SubmapEntry submap_entry;
+      submap_entry.trajectory_id = trajectory_id;
+      submap_entry.submap_index = submap_index;
       submap_entry.submap_version = submap_data.submap->num_range_data();
       submap_entry.pose = ToGeometryMsgPose(submap_data.pose);
-      trajectory_submap_list.submap.push_back(submap_entry);
+      submap_list.submap.push_back(submap_entry);
     }
-    submap_list.trajectory.push_back(trajectory_submap_list);
   }
   return submap_list;
 }

cartographer_ros/cartographer_ros/node.h

@@ -31,7 +31,6 @@
 #include "cartographer_ros_msgs/SubmapList.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "cartographer_ros_msgs/TrajectoryOptions.h"
-#include "cartographer_ros_msgs/TrajectorySubmapList.h"
 #include "cartographer_ros_msgs/WriteAssets.h"
 #include "ros/ros.h"
 #include "tf2_ros/transform_broadcaster.h"

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -244,8 +244,8 @@ void Run(const std::vector<string>& bag_filenames) {
         ::ros::spinOnce();
 
         LOG_EVERY_N(INFO, 100000)
-            << "Processed " << (delayed_msg.getTime() - begin_time).toSec() << " of "
-            << duration_in_seconds << " bag time seconds...";
+            << "Processed " << (delayed_msg.getTime() - begin_time).toSec()
+            << " of " << duration_in_seconds << " bag time seconds...";
 
         delayed_messages.pop_front();
       }
@@ -256,7 +256,6 @@ void Run(const std::vector<string>& bag_filenames) {
         continue;
       }
       delayed_messages.push_back(msg);
-
     }
 
     bag.close();

cartographer_ros_msgs/CMakeLists.txt

@@ -25,7 +25,6 @@ find_package(catkin REQUIRED COMPONENTS message_generation ${PACKAGE_DEPENDENCIE
 add_message_files(
   FILES
     SubmapList.msg
-    TrajectorySubmapList.msg
     SubmapEntry.msg
     SensorTopics.msg
     TrajectoryOptions.msg

cartographer_ros_msgs/msg/SubmapEntry.msg

@@ -12,5 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+int32 trajectory_id
+int32 submap_index
 int32 submap_version
 geometry_msgs/Pose pose

cartographer_ros_msgs/msg/SubmapList.msg

@@ -13,4 +13,4 @@
 # limitations under the License.
 
 std_msgs/Header header
-TrajectorySubmapList[] trajectory
+SubmapEntry[] submap

cartographer_ros_msgs/msg/TrajectorySubmapList.msg

@@ -1,15 +0,0 @@
-# Copyright 2016 The Cartographer Authors
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#      http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-SubmapEntry[] submap

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -72,6 +72,7 @@ class DrawableSubmap : public QObject {
 
   int submap_index() const { return submap_index_; }
   int trajectory_id() const { return trajectory_id_; }
+  int version() const { return metadata_version_; }
   bool visibility() const { return visibility_->getBool(); }
   void set_visibility(const bool visibility) {
     visibility_->setBool(visibility);

cartographer_rviz/cartographer_rviz/submaps_display.cc

@@ -19,6 +19,7 @@
 #include "OgreResourceGroupManager.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/mutex.h"
+#include "cartographer/mapping/id.h"
 #include "cartographer_ros_msgs/SubmapList.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "geometry_msgs/TransformStamped.h"
@@ -99,44 +100,61 @@ void SubmapsDisplay::reset() {
 void SubmapsDisplay::processMessage(
     const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
   ::cartographer::common::MutexLocker locker(&mutex_);
-  // In case Cartographer node is relaunched, destroy
-  // trajectories from the previous instance
-  if (msg->trajectory.size() < trajectories_.size()) {
-    trajectories_.clear();
-  }
-  for (size_t trajectory_id = 0; trajectory_id < msg->trajectory.size();
-       ++trajectory_id) {
+  // In case Cartographer node is relaunched, destroy trajectories from the
+  // previous instance.
+  for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
+    const size_t trajectory_id = submap_entry.trajectory_id;
     if (trajectory_id >= trajectories_.size()) {
-      // When a trajectory is destroyed, it also needs to delete its rviz
-      // Property object, so we use a unique_ptr for it
+      continue;
+    }
+    const auto& trajectory = trajectories_[trajectory_id].second;
+    const auto it = trajectory.find(submap_entry.submap_index);
+    if (it != trajectory.end() &&
+        it->second->version() > submap_entry.submap_version) {
+      // Versions should only increase unless Cartographer restarted.
+      trajectories_.clear();
+      break;
+    }
+  }
+  using ::cartographer::mapping::SubmapId;
+  std::set<SubmapId> listed_submaps;
+  for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
+    listed_submaps.insert(
+        SubmapId{submap_entry.trajectory_id, submap_entry.submap_index});
+    const size_t trajectory_id = submap_entry.trajectory_id;
+    while (trajectory_id >= trajectories_.size()) {
       trajectories_.push_back(Trajectory(
           ::cartographer::common::make_unique<::rviz::Property>(
               QString("Trajectory %1").arg(trajectory_id), QVariant(),
               QString("List of all submaps in Trajectory %1.")
                   .arg(trajectory_id),
               submaps_category_),
-          std::vector<std::unique_ptr<DrawableSubmap>>()));
+          std::map<int, std::unique_ptr<DrawableSubmap>>()));
     }
     auto& trajectory_category = trajectories_[trajectory_id].first;
     auto& trajectory = trajectories_[trajectory_id].second;
-    const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
-        msg->trajectory[trajectory_id].submap;
-    // Same as above, destroy the whole trajectory if we detect that
-    // we have more submaps than we should
-    if (submap_entries.size() < trajectory.size()) {
-      trajectory.clear();
-    }
-    for (size_t submap_index = 0; submap_index < submap_entries.size();
-         ++submap_index) {
-      if (submap_index >= trajectory.size()) {
-        trajectory.push_back(
+    const int submap_index = submap_entry.submap_index;
+    if (trajectory.count(submap_index) == 0) {
+      trajectory.emplace(
+          submap_index,
           ::cartographer::common::make_unique<DrawableSubmap>(
               trajectory_id, submap_index, context_->getSceneManager(),
               trajectory_category.get(), visibility_all_enabled_->getBool()));
     }
-      trajectory[submap_index]->Update(msg->header,
-                                       submap_entries[submap_index],
-                                       context_->getFrameManager());
+    trajectory.at(submap_index)
+        ->Update(msg->header, submap_entry, context_->getFrameManager());
+  }
+  // Remove all submaps not mentioned in the SubmapList.
+  for (size_t trajectory_id = 0; trajectory_id < trajectories_.size();
+       ++trajectory_id) {
+    auto& trajectory = trajectories_[trajectory_id].second;
+    for (auto it = trajectory.begin(); it != trajectory.end();) {
+      if (listed_submaps.count(
+              SubmapId{static_cast<int>(trajectory_id), it->first}) == 0) {
+        it = trajectory.erase(it);
+      } else {
+        ++it;
+      }
     }
   }
 }
@@ -150,8 +168,9 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
                                    tracking_frame_property_->getStdString(),
                                    ros::Time(0) /* latest */);
     for (auto& trajectory : trajectories_) {
-      for (auto& submap : trajectory.second) {
-        submap->SetAlpha(transform_stamped.transform.translation.z);
+      for (auto& submap_entry : trajectory.second) {
+        submap_entry.second->SetAlpha(
+            transform_stamped.transform.translation.z);
       }
     }
   } catch (const tf2::TransformException& ex) {
@@ -161,16 +180,16 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
   // Schedule fetching of new submap textures.
   for (const auto& trajectory : trajectories_) {
     int num_ongoing_requests = 0;
-    for (const auto& submap : trajectory.second) {
-      if (submap->QueryInProgress()) {
+    for (const auto& submap_entry : trajectory.second) {
+      if (submap_entry.second->QueryInProgress()) {
         ++num_ongoing_requests;
       }
     }
-    for (int submap_index = static_cast<int>(trajectory.second.size()) - 1;
-         submap_index >= 0 &&
+    for (auto it = trajectory.second.rbegin();
+         it != trajectory.second.rend() &&
          num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
-         --submap_index) {
-      if (trajectory.second[submap_index]->MaybeFetchTexture(&client_)) {
+         ++it) {
+      if (it->second->MaybeFetchTexture(&client_)) {
         ++num_ongoing_requests;
       }
     }
@@ -181,8 +200,8 @@ void SubmapsDisplay::AllEnabledToggled() {
   ::cartographer::common::MutexLocker locker(&mutex_);
   const bool visibility = visibility_all_enabled_->getBool();
   for (auto& trajectory : trajectories_) {
-    for (auto& submap : trajectory.second) {
-      submap->set_visibility(visibility);
+    for (auto& submap_entry : trajectory.second) {
+      submap_entry.second->set_visibility(visibility);
     }
   }
 }

cartographer_rviz/cartographer_rviz/submaps_display.h

@@ -17,6 +17,7 @@
 #ifndef CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_
 #define CARTOGRAPHER_RVIZ_SRC_SUBMAPS_DISPLAY_H_
 
+#include <map>
 #include <memory>
 #include <vector>
 
@@ -68,7 +69,7 @@ class SubmapsDisplay
   ::rviz::StringProperty* map_frame_property_;
   ::rviz::StringProperty* tracking_frame_property_;
   using Trajectory = std::pair<std::unique_ptr<::rviz::Property>,
-                               std::vector<std::unique_ptr<DrawableSubmap>>>;
+                               std::map<int, std::unique_ptr<DrawableSubmap>>>;
   std::vector<Trajectory> trajectories_ GUARDED_BY(mutex_);
   ::cartographer::common::Mutex mutex_;
   ::rviz::Property* submaps_category_;