Simplify the Rviz submap texture update logic. (#12)

cartographer_ros/src/drawable_submap.cc

@@ -64,11 +64,7 @@ DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id,
           kManualObjectPrefix + GetSubmapIdentifier(trajectory_id, submap_id))),
       submap_id_(submap_id),
       trajectory_id_(trajectory_id),
-      last_query_timestamp_(0),
+      last_query_timestamp_(0) {
-      query_in_progress_(false),
-      version_(0),
-      texture_count_(0),
-      current_alpha_(0) {
   material_ = Ogre::MaterialManager::getSingleton().getByName(
       kSubmapSourceMaterialName);
   material_ = material_->clone(kSubmapMaterialPrefix +
@@ -77,7 +73,7 @@ DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id,
   material_->getTechnique(0)->setLightingEnabled(false);
   material_->setDepthWriteEnabled(false);
   scene_node_->attachObject(manual_object_);
-  connect(this, SIGNAL(RequestSucceeded()), this, SLOT(OnRequestSuccess()));
+  connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
 }
 
 DrawableSubmap::~DrawableSubmap() {
@@ -90,12 +86,16 @@ DrawableSubmap::~DrawableSubmap() {
   scene_manager_->destroyManualObject(manual_object_);
 }
 
-bool DrawableSubmap::Update(
+void DrawableSubmap::Update(
-    const ::cartographer_ros_msgs::SubmapEntry& metadata,
+    const ::cartographer_ros_msgs::SubmapEntry& metadata) {
-    ros::ServiceClient* const client) {
   ::cartographer::common::MutexLocker locker(&mutex_);
   tf::poseMsgToEigen(metadata.pose, submap_pose_);
-  const bool newer_version_available = version_ < metadata.submap_version;
+  metadata_version_ = metadata.submap_version;
+}
+
+bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
+  ::cartographer::common::MutexLocker locker(&mutex_);
+  const bool newer_version_available = texture_version_ < metadata_version_;
   const std::chrono::milliseconds now =
       std::chrono::duration_cast<std::chrono::milliseconds>(
           std::chrono::system_clock::now().time_since_epoch());
@@ -106,65 +106,50 @@ bool DrawableSubmap::Update(
   }
   query_in_progress_ = true;
   last_query_timestamp_ = now;
-  last_query_slice_height_ = metadata.pose.position.z;
+  QuerySubmap(client);
-  QuerySubmap(submap_id_, trajectory_id_, client);
   return true;
 }
 
-void DrawableSubmap::QuerySubmap(const int submap_id, const int trajectory_id,
+void DrawableSubmap::QuerySubmap(ros::ServiceClient* const client) {
-                                 ros::ServiceClient* const client) {
+  rpc_request_future_ = std::async(std::launch::async, [this, client]() {
-  rpc_request_future_ = std::async(
-      std::launch::async, [this, submap_id, trajectory_id, client]() {
     ::cartographer_ros_msgs::SubmapQuery srv;
-        srv.request.submap_id = submap_id;
+    srv.request.submap_id = submap_id_;
-        srv.request.trajectory_id = trajectory_id;
+    srv.request.trajectory_id = trajectory_id_;
     if (client->call(srv)) {
-          response_.reset(
+      // We emit a signal to update in the right thread, and pass via the
-              new ::cartographer_ros_msgs::SubmapQuery::Response(srv.response));
+      // 'response_' member to simplify the signal-slot connection slightly.
+      ::cartographer::common::MutexLocker locker(&mutex_);
+      response_ = srv.response;
       Q_EMIT RequestSucceeded();
     } else {
-          OnRequestFailure();
+      ::cartographer::common::MutexLocker locker(&mutex_);
+      query_in_progress_ = false;
     }
   });
 }
 
-void DrawableSubmap::OnRequestSuccess() {
-  ::cartographer::common::MutexLocker locker(&mutex_);
-  version_ = response_->submap_version;
-  resolution_ = response_->resolution;
-  width_ = response_->width;
-  height_ = response_->height;
-  slice_height_ = last_query_slice_height_;
-  std::string compressed_cells(response_->cells.begin(),
-                               response_->cells.end());
-  cells_.clear();
-  ::cartographer::common::FastGunzipString(compressed_cells, &cells_);
-  Eigen::Affine3d slice_pose;
-  tf::poseMsgToEigen(response_->slice_pose, slice_pose);
-  tf::poseEigenToMsg(submap_pose_ * slice_pose, transformed_pose_);
-  response_.reset();
-  query_in_progress_ = false;
-  UpdateSceneNode();
-}
-
-void DrawableSubmap::OnRequestFailure() {
-  ::cartographer::common::MutexLocker locker(&mutex_);
-  query_in_progress_ = false;
-}
-
 bool DrawableSubmap::QueryInProgress() {
   ::cartographer::common::MutexLocker locker(&mutex_);
   return query_in_progress_;
 }
 
 void DrawableSubmap::UpdateSceneNode() {
+  ::cartographer::common::MutexLocker locker(&mutex_);
+  texture_version_ = response_.submap_version;
+  std::string compressed_cells(response_.cells.begin(), response_.cells.end());
+  std::string cells;
+  ::cartographer::common::FastGunzipString(compressed_cells, &cells);
+  Eigen::Affine3d slice_pose;
+  tf::poseMsgToEigen(response_.slice_pose, slice_pose);
+  tf::poseEigenToMsg(submap_pose_ * slice_pose, transformed_pose_);
+  query_in_progress_ = false;
   // The call to Ogre's loadRawData below does not work with an RG texture,
   // therefore we create an RGB one whose blue channel is always 0.
   std::vector<char> rgb;
-  for (int i = 0; i < height_; ++i) {
+  for (int i = 0; i < response_.height; ++i) {
-    for (int j = 0; j < width_; ++j) {
+    for (int j = 0; j < response_.width; ++j) {
-      auto r = cells_[(i * width_ + j) * 2];
+      auto r = cells[(i * response_.width + j) * 2];
-      auto g = cells_[(i * width_ + j) * 2 + 1];
+      auto g = cells[(i * response_.width + j) * 2 + 1];
       rgb.push_back(r);
       rgb.push_back(g);
       rgb.push_back(0.);
@@ -172,8 +157,8 @@ void DrawableSubmap::UpdateSceneNode() {
   }
 
   manual_object_->clear();
-  const float metric_width = resolution_ * width_;
+  const float metric_width = response_.resolution * response_.width;
-  const float metric_height = resolution_ * height_;
+  const float metric_height = response_.resolution * response_.height;
 
   manual_object_->begin(material_->getName(),
                         Ogre::RenderOperation::OT_TRIANGLE_LIST);
@@ -221,12 +206,11 @@ void DrawableSubmap::UpdateSceneNode() {
     texture_.setNull();
   }
   const std::string texture_name =
-      kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_id_) +
+      kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_id_);
-      std::to_string(texture_count_);
   texture_ = Ogre::TextureManager::getSingleton().loadRawData(
       texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
-      pixel_stream, width_, height_, Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D, 0);
+      pixel_stream, response_.width, response_.height, Ogre::PF_BYTE_RGB,
-  ++texture_count_;
+      Ogre::TEX_TYPE_2D, 0);
 
   Ogre::Pass* const pass = material_->getTechnique(0)->getPass(0);
   pass->setSceneBlending(Ogre::SBF_ONE, Ogre::SBF_ONE_MINUS_SOURCE_ALPHA);
@@ -248,7 +232,8 @@ void DrawableSubmap::Transform(::rviz::FrameManager* const frame_manager) {
 }
 
 void DrawableSubmap::SetAlpha(const double current_tracking_z) {
-  const double distance_z = std::abs(slice_height_ - current_tracking_z);
+  const double distance_z =
+      std::abs(submap_pose_.translation().z() - current_tracking_z);
   const double fade_distance =
       std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
   const float alpha =

cartographer_ros/src/drawable_submap.h

@@ -49,11 +49,13 @@ class DrawableSubmap : public QObject {
   DrawableSubmap(const DrawableSubmap&) = delete;
   DrawableSubmap& operator=(const DrawableSubmap&) = delete;
 
-  // 'submap_entry' contains metadata which is used to find out whether this
+  // Updates the 'metadata' for this submap. If necessary, the next call to
-  // 'DrawableSubmap' should update itself. If an update is needed, it will send
+  // MaybeFetchTexture() will fetch a new submap texture.
-  // an RPC using 'client' to request the new data for the submap.
+  void Update(const ::cartographer_ros_msgs::SubmapEntry& metadata);
-  bool Update(const ::cartographer_ros_msgs::SubmapEntry& submap_entry,
+
-              ros::ServiceClient* client);
+  // If an update is needed, it will send an RPC using 'client' to request the
+  // new data for the submap and returns true.
+  bool MaybeFetchTexture(ros::ServiceClient* client);
 
   // Returns whether an RPC is in progress.
   bool QueryInProgress();
@@ -72,13 +74,10 @@ class DrawableSubmap : public QObject {
 
  private Q_SLOTS:
   // Callback when an rpc request succeeded.
-  void OnRequestSuccess();
+  void UpdateSceneNode();
 
  private:
-  void QuerySubmap(int submap_id, int trajectory_id,
+  void QuerySubmap(ros::ServiceClient* client);
-                   ros::ServiceClient* client);
-  void OnRequestFailure();
-  void UpdateSceneNode();
   float UpdateAlpha(float target_alpha);
 
   const int submap_id_;
@@ -93,19 +92,12 @@ class DrawableSubmap : public QObject {
   Eigen::Affine3d submap_pose_ GUARDED_BY(mutex_);
   geometry_msgs::Pose transformed_pose_ GUARDED_BY(mutex_);
   std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
-  bool query_in_progress_ GUARDED_BY(mutex_);
+  bool query_in_progress_ = false GUARDED_BY(mutex_);
-  float resolution_ GUARDED_BY(mutex_);
+  int metadata_version_ = -1 GUARDED_BY(mutex_);
-  int width_ GUARDED_BY(mutex_);
+  int texture_version_ = -1 GUARDED_BY(mutex_);
-  int height_ GUARDED_BY(mutex_);
-  int version_ GUARDED_BY(mutex_);
-  double slice_height_ GUARDED_BY(mutex_);
-  double last_query_slice_height_ GUARDED_BY(mutex_);
   std::future<void> rpc_request_future_;
-  std::string cells_ GUARDED_BY(mutex_);
+  ::cartographer_ros_msgs::SubmapQuery::Response response_ GUARDED_BY(mutex_);
-  std::unique_ptr<::cartographer_ros_msgs::SubmapQuery::Response> response_
+  float current_alpha_ = 0.f;
-      GUARDED_BY(mutex_);
-  int texture_count_;
-  float current_alpha_;
 };
 
 }  // namespace rviz

cartographer_ros/src/submaps_display.cc

@@ -38,7 +38,7 @@ namespace rviz {
 
 namespace {
 
-constexpr int kMaxOnGoingRequests = 6;
+constexpr int kMaxOnGoingRequestsPerTrajectory = 6;
 constexpr char kMaterialsDirectory[] = "/ogre_media/materials";
 constexpr char kGlsl120Directory[] = "/glsl120";
 constexpr char kScriptsDirectory[] = "/scripts";
@@ -104,47 +104,22 @@ void SubmapsDisplay::processMessage(
     if (trajectory_id >= trajectories_.size()) {
       trajectories_.emplace_back();
     }
+    auto& trajectory = trajectories_[trajectory_id];
     const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
         msg->trajectory[trajectory_id].submap;
-    if (submap_entries.empty()) {
+    for (int submap_id = 0; submap_id < submap_entries.size(); ++submap_id) {
-      return;
+      if (submap_id >= trajectory.size()) {
-    }
+        trajectory.push_back(
-    for (int submap_id = trajectories_[trajectory_id].size();
-         submap_id < submap_entries.size(); ++submap_id) {
-      trajectories_[trajectory_id].push_back(
             ::cartographer::common::make_unique<DrawableSubmap>(
                 submap_id, trajectory_id, context_->getSceneManager()));
       }
-  }
+      trajectory[submap_id]->Update(submap_entries[submap_id]);
-  int num_ongoing_requests = 0;
-  for (const auto& trajectory : trajectories_) {
-    for (const auto& submap : trajectory) {
-      if (submap->QueryInProgress()) {
-        ++num_ongoing_requests;
-        if (num_ongoing_requests == kMaxOnGoingRequests) {
-          return;
-        }
-      }
-    }
-  }
-  for (int trajectory_id = 0; trajectory_id < msg->trajectory.size();
-       ++trajectory_id) {
-    const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
-        msg->trajectory[trajectory_id].submap;
-    for (int submap_id = submap_entries.size() - 1; submap_id >= 0;
-         --submap_id) {
-      if (trajectories_[trajectory_id][submap_id]->Update(
-              submap_entries[submap_id], &client_)) {
-        ++num_ongoing_requests;
-        if (num_ongoing_requests == kMaxOnGoingRequests) {
-          return;
-        }
-      }
     }
   }
 }
 
 void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
+  // Update the fading by z distance.
   try {
     const ::geometry_msgs::TransformStamped transform_stamped =
         tf_buffer_.lookupTransform(map_frame_property_->getStdString(),
@@ -159,6 +134,24 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
   } catch (const tf2::TransformException& ex) {
     ROS_WARN("Could not compute submap fading: %s", ex.what());
   }
+
+  // Schedule fetching of new submap textures.
+  for (const auto& trajectory : trajectories_) {
+    int num_ongoing_requests = 0;
+    for (const auto& submap : trajectory) {
+      if (submap->QueryInProgress()) {
+        ++num_ongoing_requests;
+      }
+    }
+    for (int submap_id = trajectory.size() - 1;
+         submap_id >= 0 &&
+         num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
+         --submap_id) {
+      if (trajectory[submap_id]->MaybeFetchTexture(&client_)) {
+        ++num_ongoing_requests;
+      }
+    }
+  }
 }
 
 void SubmapsDisplay::UpdateTransforms() {