Work in progress using the new resource retriever apis

resource_retriever_msgs/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.8)
+project(resource_retriever_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/GetResource.srv"
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

resource_retriever_msgs/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>resource_retriever_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">eloy</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

resource_retriever_msgs/srv/GetResource.srv

@@ -0,0 +1,5 @@
+string path
+---
+string path
+int32 status_code
+uint8[] body

rviz_common/src/rviz_common/load_resource.cpp

@@ -46,18 +46,11 @@
 namespace rviz_common
 {
 
-resource_retriever::MemoryResource getResource(const std::string & resource_path)
+resource_retriever::MemoryResourcePtr getResource(const std::string & resource_path)
 {
+  printf("rviz_common::getResource: %s\n", resource_path.c_str());
   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)
@@ -72,8 +65,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(resource_retriever_msgs 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
+  ${resource_retriever_msgs_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>
@@ -120,6 +121,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);
@@ -173,6 +176,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>
 
@@ -234,6 +235,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>resource_retriever_msgs</depend>
   <depend>rviz_common</depend>
   <depend>rviz_rendering</depend>
   <depend>tf2</depend>

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

@@ -32,6 +32,12 @@
 #include "rviz_default_plugins/displays/marker/marker_common.hpp"
 
 #include <memory>
+#include <rclcpp/callback_group.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/ros_integration/ros_node_abstraction_iface.hpp>
 #include <set>
 #include <sstream>
 #include <string>
@@ -47,6 +53,87 @@
 
 #include "rviz_default_plugins/displays/marker/markers/marker_factory.hpp"
 
+#include <resource_retriever_msgs/srv/get_resource.hpp>
+
+class RosRetriever: public resource_retriever::plugins::RetrieverPlugin
+{
+  using GetResource = resource_retriever_msgs::srv::GetResource;
+
+public:
+  explicit RosRetriever(rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr ros_iface):
+    ros_iface_(std::move(ros_iface))
+  {
+    auto iface = ros_iface_.lock();
+    auto raw_node = iface->get_raw_node();
+    callback_group_ = raw_node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    this->client_ = raw_node->create_client<GetResource>("/get_resource", rclcpp::ServicesQoS(), callback_group_);
+  }
+
+  ~RosRetriever() override = default;
+
+  std::string name() override
+  {
+    return "rviz_common::RosRetriever";
+  }
+
+  bool can_handle(const std::string & url) override
+  {
+    return !url.empty();
+  }
+
+  resource_retriever::MemoryResourcePtr get(const std::string & url) override
+  {
+    auto it = cached_resources_.find(url);
+    if (it != cached_resources_.end())
+    {
+      RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[Resource Cached]: %s", url.c_str());
+      return it->second;
+    }
+
+    auto node = ros_iface_.lock();
+    if (nullptr == node)
+    {
+      RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Could not acquire node lock");
+      return nullptr;
+    }
+
+    auto raw_node = node->get_raw_node();
+    if (nullptr == node->get_raw_node())
+    {
+      RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Could not acquire raw node");
+      return nullptr;
+    }
+
+    auto req = std::make_shared<GetResource::Request>();
+    req->path = url;
+    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Sending request");
+    auto result = this->client_->async_send_request(req);
+
+    auto exec = rclcpp::executors::SingleThreadedExecutor();
+    exec.add_callback_group(callback_group_, raw_node->get_node_base_interface());
+    exec.spin_until_future_complete(result);
+    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Waiting done");
+
+    auto res = result.get();
+    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Result: %s", res->path.c_str());
+
+    if (res->status_code == 0)
+    {
+     RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "[Retrieved resource]: %s", url.c_str());
+     cached_resources_.insert({url, std::make_shared<resource_retriever::MemoryResource>(url, res->path, res->body)});
+     return cached_resources_[url];
+    }
+
+    return nullptr;
+  }
+
+private:
+  rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr ros_iface_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::Client<GetResource>::SharedPtr client_;
+  std::unordered_map<std::string, resource_retriever::MemoryResourcePtr> cached_resources_;
+};
+
 namespace rviz_default_plugins
 {
 namespace displays
@@ -58,6 +145,7 @@ MarkerCommon::MarkerCommon(rviz_common::Display * display)
   namespaces_category_ = new rviz_common::properties::Property(
     "Namespaces", QVariant(), "", display_);
   marker_factory_ = std::make_unique<markers::MarkerFactory>();
+
 }
 
 MarkerCommon::~MarkerCommon()
@@ -70,6 +158,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<RosRetriever>(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_);
@@ -146,6 +242,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/robot/robot_link.cpp

@@ -91,6 +91,36 @@ using rviz_common::properties::Property;
 using rviz_common::properties::FloatProperty;
 using rviz_common::properties::QuaternionProperty;
 using rviz_common::properties::VectorProperty;
+class RosRetriever: public resource_retriever::plugins::RetrieverPlugin
+
+{
+public:
+  explicit RosRetriever(rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr ros_iface):
+    ros_iface_(std::move(ros_iface))
+  {
+  }
+
+  ~RosRetriever() override {};
+
+  std::string name() override
+  {
+    return "rviz_common::RosRetriever";
+  }
+
+  bool can_handle(const std::string & url) override
+  {
+    return true;
+  }
+
+  resource_retriever::MemoryResourcePtr get(const std::string & url) override
+  {
+    printf("Trying to get %s\n", url.c_str());
+    return nullptr;
+  }
+
+private:
+    rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr ros_iface_;
+};
 
 class RobotLinkSelectionHandler : public rviz_common::interaction::SelectionHandler
 {
@@ -225,6 +255,14 @@ RobotLink::RobotLink(
   color_material_ =
     rviz_rendering::MaterialManager::createMaterialWithLighting(color_material_name);
 
+  resource_retriever::RetrieverVec plugins;
+  plugins.push_back(std::make_shared<RosRetriever>(context_->getRosNodeAbstraction()));
+  for (const auto & plugin : resource_retriever::default_plugins())
+  {
+    plugins.push_back(plugin);
+  }
+  retriever_ = resource_retriever::Retriever(plugins);
+
   // create the ogre objects to display
   if (visual) {
     createVisual(link);
@@ -672,7 +710,7 @@ Ogre::Entity * RobotLink::createEntityForGeometryElement(
         const std::string & model_name = mesh.filename;
 
         try {
-          if (rviz_rendering::loadMeshFromResource(model_name) == nullptr) {
+          if (rviz_rendering::loadMeshFromResource(&retriever_, model_name) == nullptr) {
             addError("Could not load mesh resource '%s'", model_name.c_str());
           } else {
             entity = scene_manager_->createEntity(entity_name, model_name);
@@ -800,16 +838,9 @@ void RobotLink::loadMaterialFromTexture(
 {
   std::string filename = visual->material->texture_filename;
   if (!Ogre::TextureManager::getSingleton().resourceExists(filename, RVIZ_RESOURCE_GROUP)) {
-    resource_retriever::Retriever retriever;
-    resource_retriever::MemoryResource res;
-    try {
-      res = retriever.get(filename);
-    } catch (resource_retriever::Exception & e) {
-      RVIZ_COMMON_LOG_ERROR(e.what());
-    }
-
-    if (res.size != 0) {
-      Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(res.data.get(), res.size));
+    auto res = retriever_.get(filename);
+    if (nullptr != res && !res->data.empty()) {
+      Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(const_cast<uint8_t*>(res->data.data()), res->data.size()));
       Ogre::Image image;
       std::string extension =
         QFileInfo(QString::fromStdString(filename)).completeSuffix().toStdString();

rviz_default_plugins/test/rviz_default_plugins/robot/robot_test.cpp

@@ -84,8 +84,8 @@ class RobotTestFixture : public Test
     EXPECT_CALL(*context_, getHandlerManager()).WillRepeatedly(Return(handle_manager_));
 
     resource_retriever::Retriever retriever;
-    auto file = retriever.get("package://rviz_default_plugins/test_meshes/test.urdf");
-    std::string string(reinterpret_cast<char *>(file.data.get()), file.size);
+    auto res = retriever.get("package://rviz_default_plugins/test_meshes/test.urdf");
+    std::string string(reinterpret_cast<char *>(const_cast<uint8_t*>(res->data.data())), res->data.size());
     urdf_model_.initString(string);
 
     parent_ = std::make_shared<rviz_common::properties::Property>();