Pull out FetchSubmapTexture into cartographer_ros. (#433)

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -55,4 +55,3 @@ install(TARGETS cartographer_start_trajectory
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
-

cartographer_ros/cartographer_ros/node.cc

@@ -50,9 +50,6 @@ using carto::transform::Rigid3d;
 
 namespace {
 
-constexpr int kInfiniteSubscriberQueueSize = 0;
-constexpr int kLatestOnlyPublisherQueueSize = 1;
-
 void ShutdownSubscriber(std::unordered_map<int, ::ros::Subscriber>& subscribers,
                         int trajectory_id) {
   if (subscribers.count(trajectory_id) == 0) {

cartographer_ros/cartographer_ros/node_constants.h

@@ -36,6 +36,9 @@ constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
 constexpr char kConstraintListTopic[] = "constraint_list";
 constexpr double kConstraintPublishPeriodSec = 0.5;
 
+constexpr int kInfiniteSubscriberQueueSize = 0;
+constexpr int kLatestOnlyPublisherQueueSize = 1;
+
 }  // namespace cartographer_ros
 
 #endif  // CARTOGRAPHER_ROS_NODE_CONSTANTS_H_

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -61,7 +61,6 @@ namespace {
 constexpr char kClockTopic[] = "clock";
 constexpr char kTfStaticTopic[] = "/tf_static";
 constexpr char kTfTopic[] = "tf";
-constexpr int kLatestOnlyPublisherQueueSize = 1;
 
 volatile std::sig_atomic_t sigint_triggered = 0;
 

cartographer_ros/cartographer_ros/submap.cc

@@ -0,0 +1,58 @@
+/*
+ * 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.
+ */
+
+#include "cartographer_ros/submap.h"
+
+#include "cartographer/common/make_unique.h"
+#include "cartographer/common/port.h"
+#include "cartographer/transform/transform.h"
+#include "cartographer_ros/msg_conversion.h"
+#include "cartographer_ros_msgs/SubmapQuery.h"
+
+namespace cartographer_ros {
+
+std::unique_ptr<SubmapTexture> FetchSubmapTexture(
+    const ::cartographer::mapping::SubmapId& submap_id,
+    ros::ServiceClient* client) {
+  ::cartographer_ros_msgs::SubmapQuery srv;
+  srv.request.trajectory_id = submap_id.trajectory_id;
+  srv.request.submap_index = submap_id.submap_index;
+  if (!client->call(srv)) {
+    return nullptr;
+  }
+  std::string compressed_cells(srv.response.cells.begin(),
+                               srv.response.cells.end());
+  std::string cells;
+  ::cartographer::common::FastGunzipString(compressed_cells, &cells);
+  const int num_pixels = srv.response.width * srv.response.height;
+  CHECK_EQ(cells.size(), 2 * num_pixels);
+  std::vector<char> intensity;
+  intensity.reserve(num_pixels);
+  std::vector<char> alpha;
+  alpha.reserve(num_pixels);
+  for (int i = 0; i < srv.response.height; ++i) {
+    for (int j = 0; j < srv.response.width; ++j) {
+      intensity.push_back(cells[(i * srv.response.width + j) * 2]);
+      alpha.push_back(cells[(i * srv.response.width + j) * 2 + 1]);
+    }
+  }
+  return ::cartographer::common::make_unique<SubmapTexture>(SubmapTexture{
+      srv.response.submap_version, intensity, alpha, srv.response.width,
+      srv.response.height, srv.response.resolution,
+      ToRigid3d(srv.response.slice_pose)});
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/submap.h

@@ -0,0 +1,47 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_SUBMAP_H_
+#define CARTOGRAPHER_ROS_SUBMAP_H_
+
+#include <memory>
+#include <vector>
+
+#include "cartographer/mapping/id.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "ros/ros.h"
+
+namespace cartographer_ros {
+
+struct SubmapTexture {
+  int version;
+  std::vector<char> intensity;
+  std::vector<char> alpha;
+  int width;
+  int height;
+  double resolution;
+  ::cartographer::transform::Rigid3d slice_pose;
+};
+
+// Fetch 'submap_id' using the 'client' and returning the response or 'nullptr'
+// on error.
+std::unique_ptr<SubmapTexture> FetchSubmapTexture(
+    const ::cartographer::mapping::SubmapId& submap_id,
+    ros::ServiceClient* client);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_SUBMAP_H_

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -97,6 +97,8 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
 }
 
 DrawableSubmap::~DrawableSubmap() {
+  // 'query_in_progress_' must be true until the Q_EMIT has happened. Qt then
+  // makes sure that 'RequestSucceeded' is not called after our destruction.
   if (QueryInProgress()) {
     rpc_request_future_.wait();
   }
@@ -120,10 +122,9 @@ void DrawableSubmap::Update(
     return;
   }
   ::cartographer::common::MutexLocker locker(&mutex_);
-  submap_z_ = metadata.pose.position.z;
   metadata_version_ = metadata.submap_version;
   pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
-  if (texture_version_ != -1) {
+  if (submap_texture_ != nullptr) {
     // We have to update the transform since we are already displaying a texture
     // for this submap.
     UpdateTransform();
@@ -140,8 +141,10 @@ void DrawableSubmap::Update(
 
 bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
   ::cartographer::common::MutexLocker locker(&mutex_);
-  // Received metadata version can also be lower - if we restarted Cartographer
-  const bool newer_version_available = texture_version_ != metadata_version_;
+  // Received metadata version can also be lower if we restarted Cartographer.
+  const bool newer_version_available =
+      submap_texture_ == nullptr ||
+      submap_texture_->version != metadata_version_;
   const std::chrono::milliseconds now =
       std::chrono::duration_cast<std::chrono::milliseconds>(
           std::chrono::system_clock::now().time_since_epoch());
@@ -153,18 +156,16 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
   query_in_progress_ = true;
   last_query_timestamp_ = now;
   rpc_request_future_ = std::async(std::launch::async, [this, client]() {
-    ::cartographer_ros_msgs::SubmapQuery srv;
-    srv.request.trajectory_id = id_.trajectory_id;
-    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.
-      ::cartographer::common::MutexLocker locker(&mutex_);
-      response_ = srv.response;
-      Q_EMIT RequestSucceeded();
-    } else {
+    std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture =
+        ::cartographer_ros::FetchSubmapTexture(id_, client);
     ::cartographer::common::MutexLocker locker(&mutex_);
     query_in_progress_ = false;
+    if (submap_texture != nullptr) {
+      // We emit a signal to update in the right thread, and pass via the
+      // 'submap_texture_' member to simplify the signal-slot connection
+      // slightly.
+      submap_texture_ = std::move(submap_texture);
+      Q_EMIT RequestSucceeded();
     }
   });
   return true;
@@ -176,7 +177,8 @@ bool DrawableSubmap::QueryInProgress() {
 }
 
 void DrawableSubmap::SetAlpha(const double current_tracking_z) {
-  const double distance_z = std::abs(submap_z_ - current_tracking_z);
+  const double distance_z =
+      std::abs(pose_.translation().z() - current_tracking_z);
   const double fade_distance =
       std::max(distance_z - kFadeOutStartDistanceInMeters, 0.);
   const float alpha = static_cast<float>(
@@ -189,29 +191,21 @@ void DrawableSubmap::SetAlpha(const double current_tracking_z) {
 
 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);
-  slice_pose_ = ::cartographer_ros::ToRigid3d(response_.slice_pose);
   UpdateTransform();
-  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 < response_.height; ++i) {
-    for (int j = 0; j < response_.width; ++j) {
-      auto r = cells[(i * response_.width + j) * 2];
-      auto g = cells[(i * response_.width + j) * 2 + 1];
-      rgb.push_back(r);
-      rgb.push_back(g);
+  for (size_t i = 0; i < submap_texture_->intensity.size(); ++i) {
+    rgb.push_back(submap_texture_->intensity[i]);
+    rgb.push_back(submap_texture_->alpha[i]);
     rgb.push_back(0.);
   }
-  }
 
   manual_object_->clear();
-  const float metric_width = response_.resolution * response_.width;
-  const float metric_height = response_.resolution * response_.height;
+  const float metric_width =
+      submap_texture_->resolution * submap_texture_->width;
+  const float metric_height =
+      submap_texture_->resolution * submap_texture_->height;
   manual_object_->begin(material_->getName(),
                         Ogre::RenderOperation::OT_TRIANGLE_STRIP);
   // Bottom left
@@ -239,8 +233,8 @@ void DrawableSubmap::UpdateSceneNode() {
       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,
-      Ogre::TEX_TYPE_2D, 0);
+      pixel_stream, submap_texture_->width, submap_texture_->height,
+      Ogre::PF_BYTE_RGB, 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);
@@ -253,7 +247,9 @@ void DrawableSubmap::UpdateSceneNode() {
 }
 
 void DrawableSubmap::UpdateTransform() {
-  const ::cartographer::transform::Rigid3d pose = pose_ * slice_pose_;
+  CHECK(submap_texture_ != nullptr);
+  const ::cartographer::transform::Rigid3d pose =
+      pose_ * submap_texture_->slice_pose;
   scene_node_->setPosition(ToOgre(pose.translation()));
   scene_node_->setOrientation(ToOgre(pose.rotation()));
 }

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -18,6 +18,7 @@
 #define CARTOGRAPHER_RVIZ_SRC_DRAWABLE_SUBMAP_H_
 
 #include <future>
+#include <memory>
 
 #include "Eigen/Core"
 #include "Eigen/Geometry"
@@ -31,6 +32,7 @@
 #include "cartographer/common/mutex.h"
 #include "cartographer/mapping/id.h"
 #include "cartographer/transform/rigid_transform.h"
+#include "cartographer_ros/submap.h"
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "ros/ros.h"
@@ -99,15 +101,13 @@ class DrawableSubmap : public QObject {
   Ogre::ManualObject* manual_object_;
   Ogre::TexturePtr texture_;
   Ogre::MaterialPtr material_;
-  double submap_z_ = 0. GUARDED_BY(mutex_);
   ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
-  ::cartographer::transform::Rigid3d slice_pose_ GUARDED_BY(mutex_);
   std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
   bool query_in_progress_ = false GUARDED_BY(mutex_);
   int metadata_version_ = -1 GUARDED_BY(mutex_);
-  int texture_version_ = -1 GUARDED_BY(mutex_);
   std::future<void> rpc_request_future_;
-  ::cartographer_ros_msgs::SubmapQuery::Response response_ GUARDED_BY(mutex_);
+  std::unique_ptr<::cartographer_ros::SubmapTexture> submap_texture_
+      GUARDED_BY(mutex_);
   float current_alpha_ = 0.f;
   std::unique_ptr<::rviz::BoolProperty> visibility_;
 };