Work in progress using the new resource retriever apis

rviz_common/src/rviz_common/load_resource.cpp

@@ -47,18 +47,11 @@
 namespace rviz_common
 {
 
-resource_retriever::MemoryResource getResource(const std::string & resource_path)
+resource_retriever::MemoryResourcePtr getResource(const std::string & resource_path)
 {
+  RVIZ_COMMON_LOG_DEBUG("rviz_common::getResource() loading resource: " + resource_path);
   resource_retriever::Retriever retriever;
-  resource_retriever::MemoryResource res;
-  try {
-    res = retriever.get(resource_path);
-  } catch (resource_retriever::Exception & e) {
-    RVIZ_COMMON_LOG_DEBUG(e.what());
-    return resource_retriever::MemoryResource();
-  }
-
-  return res;
+  return retriever.get(resource_path);
 }
 
 QPixmap loadPixmap(QString url, bool fill_cache)
@@ -73,8 +66,8 @@ QPixmap loadPixmap(QString url, bool fill_cache)
   RVIZ_COMMON_LOG_DEBUG("Load pixmap at " + url.toStdString());
 
   auto image = getResource(url.toStdString());
-  if (image.size != 0) {
-    if (!pixmap.loadFromData(image.data.get(), static_cast<uint32_t>(image.size))) {
+  if (image!= nullptr && !image->data.empty()) {
+    if (!pixmap.loadFromData(image->data.data(), static_cast<uint32_t>(image->data.size()))) {
       RVIZ_COMMON_LOG_ERROR("Could not load pixmap " + url.toStdString());
     }
   }

rviz_default_plugins/CMakeLists.txt

@@ -73,6 +73,7 @@ find_package(pluginlib REQUIRED)
 find_package(point_cloud_transport REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(resource_retriever REQUIRED)
+find_package(rviz_resource_interfaces REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
@@ -262,11 +263,12 @@ target_link_libraries(rviz_default_plugins PUBLIC
   tf2_ros::tf2_ros
   urdf::urdf
   ${visualization_msgs_TARGETS}
+  resource_retriever::resource_retriever
+  ${rviz_resource_interfaces_TARGETS}
 )
 
 target_link_libraries(rviz_default_plugins PRIVATE
   gz-math::core
-  resource_retriever::resource_retriever
 )
 
 # Causes the visibility macros to use dllexport rather than dllimport,

rviz_default_plugins/include/rviz_default_plugins/displays/marker/marker_common.hpp

@@ -35,6 +35,7 @@
 #include <map>
 #include <memory>
 #include <mutex>
+#include <resource_retriever/retriever.hpp>
 #include <set>
 #include <string>
 #include <vector>
@@ -122,6 +123,8 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerCommon
   void setMarkerStatus(MarkerID id, StatusLevel level, const std::string & text);
   void deleteMarkerStatus(MarkerID id);
 
+  resource_retriever::Retriever * getResourceRetriever();
+
 private:
   /** @brief Delete all the markers within the given namespace. */
   void deleteMarkersInNamespace(const std::string & ns);
@@ -175,6 +178,8 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC MarkerCommon
   rviz_common::DisplayContext * context_;
   Ogre::SceneNode * scene_node_;
 
+  resource_retriever::Retriever retriever_;
+
   friend class MarkerNamespace;
 };
 

rviz_default_plugins/include/rviz_default_plugins/robot/robot_link.hpp

@@ -34,6 +34,7 @@
 
 #include <map>
 #include <memory>
+#include <resource_retriever/retriever.hpp>
 #include <string>
 #include <vector>
 
@@ -235,6 +236,8 @@ private Q_SLOTS:
   Ogre::SceneManager * scene_manager_;
   rviz_common::DisplayContext * context_;
 
+  mutable resource_retriever::Retriever retriever_;
+
   std::string parent_joint_name_;
   std::vector<std::string> child_joint_names_;
 

rviz_default_plugins/package.xml

@@ -48,6 +48,7 @@
   <depend>point_cloud_transport</depend>
   <depend>rclcpp</depend>
   <depend>resource_retriever</depend>
+  <depend>rviz_resource_interfaces</depend>
   <depend>rviz_common</depend>
   <depend>rviz_rendering</depend>
   <depend>tf2</depend>

rviz_default_plugins/src/rviz_default_plugins/displays/marker/marker_common.cpp

@@ -31,23 +31,182 @@
 
 #include "rviz_default_plugins/displays/marker/marker_common.hpp"
 
+#include <cinttypes>
 #include <memory>
 #include <set>
 #include <sstream>
 #include <string>
+#include <string_view>
 #include <utility>
 #include <vector>
 
-#include <QString>  // NOLINT: cpplint is unable to handle the include order here
+#include <QString>
+#include <rclcpp/callback_group.hpp>
+#include <rclcpp/duration.hpp>
+#include <rclcpp/executors/single_threaded_executor.hpp>
+#include <resource_retriever/memory_resource.hpp>
+#include <resource_retriever/plugins/retriever_plugin.hpp>
+#include <resource_retriever/retriever.hpp>
+#include <rviz_common/display.hpp>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/properties/property.hpp>
+#include <rviz_common/ros_integration/ros_node_abstraction_iface.hpp>
+#include <rviz_common/validate_floats.hpp>
+#include <rviz_resource_interfaces/srv/get_resource.hpp>
 
-#include "rclcpp/duration.hpp"
+#include "rviz_default_plugins/displays/marker/markers/marker_factory.hpp"
 
-#include "rviz_common/display.hpp"
-#include "rviz_common/display_context.hpp"
-#include "rviz_common/properties/property.hpp"
-#include "rviz_common/validate_floats.hpp"
+rclcpp::Node::SharedPtr
+get_ros_node_from(
+  const rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr & weak_ros_iface)
+{
+  auto ros_iface = weak_ros_iface.lock();
+  if (!ros_iface) {
+    throw std::invalid_argument("ROS node abstraction interface not valid");
+  }
+  return ros_iface->get_raw_node();
+}
 
-#include "rviz_default_plugins/displays/marker/markers/marker_factory.hpp"
+class RosResourceRetriever : public resource_retriever::plugins::RetrieverPlugin
+{
+  using GetResource = rviz_resource_interfaces::srv::GetResource;
+
+  RosResourceRetriever() = delete;
+
+  static constexpr std::string_view service_name = "/rviz/get_resource";
+
+public:
+  explicit RosResourceRetriever(
+    rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr weak_ros_iface)
+  : ros_node_(get_ros_node_from(weak_ros_iface)),
+    logger_(ros_node_->get_logger().get_child("ros_resource_retriever"))
+  {
+    this->logger_ = ros_node_->get_logger().get_child("ros_resource_retriever");
+
+    // Create a client with a custom callback group that will not be included in the main executor.
+    callback_group_ = ros_node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive,
+      false);
+    this->client_ = ros_node_->create_client<GetResource>(
+      service_name.data(),
+      rclcpp::ServicesQoS(),
+      callback_group_);
+
+    // Add the callback group to the executor so we can spin on it later.
+    executor_.add_callback_group(callback_group_, ros_node_->get_node_base_interface());
+  }
+
+  ~RosResourceRetriever() override = default;
+
+  std::string name() override
+  {
+    return "rviz_common::RosResourceRetriever";
+  }
+
+  bool can_handle(const std::string & url) override
+  {
+    return !url.empty();
+  }
+
+  resource_retriever::MemoryResourcePtr get(const std::string & url) override
+  {
+    RCLCPP_DEBUG(this->logger_, "Getting resource: %s", url.c_str());
+
+    // First check for a cache hit.
+    std::string etag;
+    auto it = cached_resources_.find(url);
+    if (it != cached_resources_.end())
+    {
+      etag = it->second.first;
+      // If the etag was not set, then the server doesn't do caching, just return what we have.
+      if (etag.empty()) {
+        RCLCPP_DEBUG(this->logger_, "Resource '%s' cached without etag, returning.", url.c_str());
+        return it->second.second;
+      }
+    }
+
+    // Request the resource with an etag, if it is set.
+    RCLCPP_DEBUG(
+      this->logger_,
+      "Requesting resource '%s'%s.",
+      url.c_str(),
+      etag.empty() ? "" : (" with etag '" + etag + "'").c_str());
+    auto req = std::make_shared<GetResource::Request>();
+    req->path = url;
+    req->etag = etag;
+    auto result = this->client_->async_send_request(req);
+    executor_.spin_until_future_complete(result);
+
+    auto res = result.get();
+    std::shared_ptr<resource_retriever::MemoryResource> memory_resource = nullptr;
+    switch (res->status_code) {
+      case rviz_resource_interfaces::srv::GetResource::Response::OK:
+        RCLCPP_DEBUG(
+          this->logger_,
+          "Received resource '%s' with etag '%s', caching and returning %zu bytes.",
+          res->expanded_path.c_str(),
+          res->etag.c_str(),
+          res->body.size());
+        memory_resource =
+          std::make_shared<resource_retriever::MemoryResource>(url, res->expanded_path, res->body);
+        cached_resources_.insert({url, {res->etag, memory_resource}});
+        return memory_resource;
+      case rviz_resource_interfaces::srv::GetResource::Response::NOT_MODIFIED:
+        RCLCPP_DEBUG(
+          this->logger_,
+          "Resource '%s' with etag '%s' was not modified, returning cached value.",
+          res->expanded_path.c_str(),
+          res->etag.c_str());
+        if (etag != res->etag) {
+          RCLCPP_WARN(
+            this->logger_,
+            "Unexpectedly got a different etag values ('%s' vs '%s') for resource '%s' "
+            "with a NOT_MODIFIED status_code. This will not stop the resource "
+            "from loading, but indicates some issue with the caching logic.",
+            res->expanded_path.c_str(),
+            etag.c_str(),
+            res->etag.c_str());
+        }
+        return it->second.second;
+        break;
+      case rviz_resource_interfaces::srv::GetResource::Response::ERROR:
+        RCLCPP_DEBUG(
+          this->logger_,
+          "Received an unexpected error when getting resource '%s': %s",
+          url.c_str(),
+          res->error_reason.c_str());
+        return nullptr;
+        break;
+      default:
+        RCLCPP_ERROR(
+          this->logger_,
+          "Unexpected status_code from resource ROS Service '%s' for resource '%s': %" PRId32,
+          service_name.data(),
+          url.c_str(),
+          res->status_code);
+        return nullptr;
+        break;
+    };
+  }
+
+private:
+  // It should be safe to keep a shared pointer to the node here, because this
+  // plugin will be destroyed with the resource retriever in the marker display,
+  // which should be destroyed along before the node abstraction is destroyed.
+  // Also, since we're keeping callback groups and clients around, we need to
+  // ensure the node stays around too.
+  rclcpp::Node::SharedPtr ros_node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::Client<GetResource>::SharedPtr client_;
+  rclcpp::executors::SingleThreadedExecutor executor_;
+  rclcpp::Logger logger_;
+
+  // Map of the resource path to a pair with the etag value and the memory resource that is cached.
+  std::unordered_map<
+    std::string,
+    std::pair<std::string, resource_retriever::MemoryResourcePtr>
+  > cached_resources_;
+};
 
 namespace rviz_default_plugins
 {
@@ -72,6 +231,14 @@ void MarkerCommon::initialize(rviz_common::DisplayContext * context, Ogre::Scene
   context_ = context;
   scene_node_ = scene_node;
 
+  resource_retriever::RetrieverVec plugins;
+  plugins.push_back(std::make_shared<RosResourceRetriever>(context_->getRosNodeAbstraction()));
+  for (const auto & plugin : resource_retriever::default_plugins())
+  {
+    plugins.push_back(plugin);
+  }
+  retriever_ = resource_retriever::Retriever(plugins);
+
   namespace_config_enabled_state_.clear();
 
   marker_factory_->initialize(this, context_, scene_node_);
@@ -148,6 +315,11 @@ void MarkerCommon::deleteMarkerStatus(MarkerID id)
   display_->deleteStatusStd(marker_name);
 }
 
+resource_retriever::Retriever * MarkerCommon::getResourceRetriever()
+{
+  return &this->retriever_;
+}
+
 void MarkerCommon::addMessage(const visualization_msgs::msg::Marker::ConstSharedPtr marker)
 {
   std::unique_lock<std::mutex> lock(queue_mutex_);

rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/mesh_resource_marker.cpp

@@ -109,7 +109,7 @@ void MeshResourceMarker::onNewMessage(
       return;
     }
 
-    if (!rviz_rendering::loadMeshFromResource(new_message->mesh_resource)) {
+    if (!rviz_rendering::loadMeshFromResource(owner_->getResourceRetriever(), new_message->mesh_resource)) {
       printMeshLoadingError(new_message);
       return;
     }

rviz_default_plugins/src/rviz_default_plugins/displays/marker/markers/triangle_list_marker.cpp

@@ -44,7 +44,6 @@
 #include <OgreTextureManager.h>
 #include <OgreTechnique.h>
 
-#include "resource_retriever/retriever.hpp"
 #include "rviz_rendering/mesh_loader.hpp"
 #include "rviz_rendering/material_manager.hpp"
 #include "rviz_common/display_context.hpp"