Use SubmapId in cartographer_rviz. (#419)

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -47,30 +47,28 @@ constexpr double kFadeOutStartDistanceInMeters = 1.;
 constexpr double kFadeOutDistanceInMeters = 2.;
 constexpr float kAlphaUpdateThreshold = 0.2f;
 
-std::string GetSubmapIdentifier(const int trajectory_id,
+std::string GetSubmapIdentifier(
-                                const int submap_index) {
+    const ::cartographer::mapping::SubmapId& submap_id) {
-  return std::to_string(trajectory_id) + "-" + std::to_string(submap_index);
+  return std::to_string(submap_id.trajectory_id) + "-" +
+         std::to_string(submap_id.submap_index);
 }
 
 }  // namespace
 
-DrawableSubmap::DrawableSubmap(const int trajectory_id, const int submap_index,
+DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
                                Ogre::SceneManager* const scene_manager,
                                ::rviz::Property* submap_category,
                                const bool visible)
-    : trajectory_id_(trajectory_id),
+    : id_(id),
-      submap_index_(submap_index),
       scene_manager_(scene_manager),
       scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()),
       manual_object_(scene_manager->createManualObject(
-          kManualObjectPrefix +
+          kManualObjectPrefix + GetSubmapIdentifier(id))),
-          GetSubmapIdentifier(trajectory_id, submap_index))),
       last_query_timestamp_(0) {
   material_ = Ogre::MaterialManager::getSingleton().getByName(
       kSubmapSourceMaterialName);
   material_ =
-      material_->clone(kSubmapMaterialPrefix +
+      material_->clone(kSubmapMaterialPrefix + GetSubmapIdentifier(id_));
-                       GetSubmapIdentifier(trajectory_id_, submap_index));
   material_->setReceiveShadows(false);
   material_->getTechnique(0)->setLightingEnabled(false);
   material_->setCullingMode(Ogre::CULL_NONE);
@@ -123,12 +121,12 @@ void DrawableSubmap::Update(
     UpdateTransform();
   }
   visibility_->setName(
-      QString("%1.%2").arg(submap_index_).arg(metadata_version_));
+      QString("%1.%2").arg(id_.submap_index).arg(metadata_version_));
   visibility_->setDescription(
       QString("Toggle visibility of this individual submap.<br><br>"
               "Trajectory %1, submap %2, submap version %3")
-          .arg(trajectory_id_)
+          .arg(id_.trajectory_id)
-          .arg(submap_index_)
+          .arg(id_.submap_index)
           .arg(metadata_version_));
 }
 
@@ -148,8 +146,8 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
   last_query_timestamp_ = now;
   rpc_request_future_ = std::async(std::launch::async, [this, client]() {
     ::cartographer_ros_msgs::SubmapQuery srv;
-    srv.request.trajectory_id = trajectory_id_;
+    srv.request.trajectory_id = id_.trajectory_id;
-    srv.request.submap_index = submap_index_;
+    srv.request.submap_index = id_.submap_index;
     if (client->call(srv)) {
       // We emit a signal to update in the right thread, and pass via the
       // 'response_' member to simplify the signal-slot connection slightly.
@@ -230,7 +228,7 @@ void DrawableSubmap::UpdateSceneNode() {
     texture_.setNull();
   }
   const std::string texture_name =
-      kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_index_);
+      kSubmapTexturePrefix + GetSubmapIdentifier(id_);
   texture_ = Ogre::TextureManager::getSingleton().loadRawData(
       texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
       pixel_stream, response_.width, response_.height, Ogre::PF_BYTE_RGB,

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -29,6 +29,7 @@
 #include "OgreTexture.h"
 #include "OgreVector3.h"
 #include "cartographer/common/mutex.h"
+#include "cartographer/mapping/id.h"
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "ros/ros.h"
@@ -44,9 +45,8 @@ class DrawableSubmap : public QObject {
   Q_OBJECT
 
  public:
-  // Each submap is identified by a 'trajectory_id' plus a 'submap_index'.
+  // The 'scene_manager' is the Ogre scene manager to which to add a node.
-  // 'scene_manager' is the Ogre scene manager to which to add a node.
+  DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id,
-  DrawableSubmap(int trajectory_id, int submap_index,
                  Ogre::SceneManager* scene_manager,
                  ::rviz::Property* submap_category, const bool visible);
   ~DrawableSubmap() override;
@@ -70,8 +70,7 @@ class DrawableSubmap : public QObject {
   // 'current_tracking_z'.
   void SetAlpha(double current_tracking_z);
 
-  int submap_index() const { return submap_index_; }
+  ::cartographer::mapping::SubmapId id() const { return id_; }
-  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) {
@@ -91,8 +90,7 @@ class DrawableSubmap : public QObject {
   void UpdateTransform();
   float UpdateAlpha(float target_alpha);
 
-  const int trajectory_id_;
+  const ::cartographer::mapping::SubmapId id_;
-  const int submap_index_;
 
   ::cartographer::common::Mutex mutex_;
   Ogre::SceneManager* const scene_manager_;

cartographer_rviz/cartographer_rviz/submaps_display.cc

@@ -119,29 +119,27 @@ void SubmapsDisplay::processMessage(
   using ::cartographer::mapping::SubmapId;
   std::set<SubmapId> listed_submaps;
   for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
-    listed_submaps.insert(
+    const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index};
-        SubmapId{submap_entry.trajectory_id, submap_entry.submap_index});
+    listed_submaps.insert(id);
-    const size_t trajectory_id = submap_entry.trajectory_id;
+    while (id.trajectory_id >= static_cast<int>(trajectories_.size())) {
-    while (trajectory_id >= trajectories_.size()) {
       trajectories_.push_back(Trajectory(
           ::cartographer::common::make_unique<::rviz::Property>(
-              QString("Trajectory %1").arg(trajectory_id), QVariant(),
+              QString("Trajectory %1").arg(id.trajectory_id), QVariant(),
               QString("List of all submaps in Trajectory %1.")
-                  .arg(trajectory_id),
+                  .arg(id.trajectory_id),
               submaps_category_),
           std::map<int, std::unique_ptr<DrawableSubmap>>()));
     }
-    auto& trajectory_category = trajectories_[trajectory_id].first;
+    auto& trajectory_category = trajectories_[id.trajectory_id].first;
-    auto& trajectory = trajectories_[trajectory_id].second;
+    auto& trajectory = trajectories_[id.trajectory_id].second;
-    const int submap_index = submap_entry.submap_index;
+    if (trajectory.count(id.submap_index) == 0) {
-    if (trajectory.count(submap_index) == 0) {
       trajectory.emplace(
-          submap_index,
+          id.submap_index,
           ::cartographer::common::make_unique<DrawableSubmap>(
-              trajectory_id, submap_index, context_->getSceneManager(),
+              id, context_->getSceneManager(), trajectory_category.get(),
-              trajectory_category.get(), visibility_all_enabled_->getBool()));
+              visibility_all_enabled_->getBool()));
     }
-    trajectory.at(submap_index)
+    trajectory.at(id.submap_index)
         ->Update(msg->header, submap_entry, context_->getFrameManager());
   }
   // Remove all submaps not mentioned in the SubmapList.