use a common object info cache for the serviec and the RViz plugin

CMakeLists.txt

@@ -35,6 +35,7 @@ catkin_python_setup()
 
 # build some cells
 include_directories(SYSTEM ${catkin_INCLUDE_DIRS})
+include_directories(include)
 
 add_subdirectory(python)
 
@@ -83,3 +84,8 @@ install(DIRECTORY launch
 install(DIRECTORY ${PROJECT_SOURCE_DIR}/conf
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+# install the include dirs
+install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)

include/object_recognition_ros/object_info_cache.h

@@ -0,0 +1,93 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef ORK_OBJECT_INFO_CACHE_H_
+#define ORK_OBJECT_INFO_CACHE_H_
+
+#include <string>
+
+#include <pluginlib/class_loader.h>
+
+#include <object_recognition_core/db/prototypes/object_info.h>
+#include <object_recognition_msgs/ObjectInformation.h>
+#include <object_recognition_msgs/ObjectType.h>
+
+namespace object_recognition_ros {
+
+class ObjectInfoCache {
+ public:
+  ObjectInfoCache();
+
+ protected:
+  void
+  getInfoBase(const object_recognition_msgs::ObjectType & type,
+              object_recognition_core::prototypes::ObjectInfo &info);
+
+ private:
+  /** Loader for the custom DB classes */
+  boost::shared_ptr<
+      pluginlib::ClassLoader<object_recognition_core::db::ObjectDb> > db_class_loader_;
+  /** Temporary storage for the loaded DB's (for reusability) */
+  std::map<std::string, object_recognition_core::db::ObjectDbPtr> db_loaded_;
+  /** Keep track of the RViz resources containing the meshes retrieved for the DB */
+  std::map<std::string, object_recognition_core::prototypes::ObjectInfo> object_informations_;
+};
+
+class ObjectInfoDiskCache : public ObjectInfoCache {
+ public:
+  ~ObjectInfoDiskCache();
+
+  void
+  getInfo(const object_recognition_msgs::ObjectType & type,
+          object_recognition_core::prototypes::ObjectInfo &info);
+
+ private:
+  /** Keep track of the files loaded from the DB and stored locally in a tmp file */
+  std::map<std::string, std::string> mesh_files_;
+};
+
+class ObjectInfoRamCache : public ObjectInfoCache {
+ public:
+  void
+  getInfo(const object_recognition_msgs::ObjectType & type,
+          object_recognition_msgs::ObjectInformation &info);
+
+ private:
+  /** Keep track of the RViz resources containing the meshes retrieved for the DB */
+  std::map<std::string, object_recognition_msgs::ObjectInformation> object_informations_;
+};
+}
+
+#endif /* ORK_OBJECT_INFO_CACHE_H_ */

src/CMakeLists.txt

@@ -1,4 +1,5 @@
 if (${ROS_GROOVY_OR_ABOVE_FOUND})
+add_subdirectory(info_cache)
 add_subdirectory(info_service)
 add_subdirectory(rviz_plugin)
 endif()

src/info_cache/CMakeLists.txt

@@ -0,0 +1,5 @@
+add_library(object_information_cache info_cache.cpp)
+target_link_libraries(object_information_cache ${catkin_LIBRARIES})
+
+install(TARGETS object_information_cache
+  DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

src/info_cache/info_cache.cpp

@@ -0,0 +1,175 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2012, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <fstream>
+#include <sstream>
+#include <stdio.h>
+
+#include <geometric_shapes/shape_operations.h>
+
+#include <object_recognition_ros/object_info_cache.h>
+
+namespace object_recognition_ros {
+
+ObjectInfoCache::ObjectInfoCache() {
+  db_class_loader_.reset(
+      new pluginlib::ClassLoader<object_recognition_core::db::ObjectDb>(
+          "object_recognition_core", "object_recognition_core::db::ObjectDb"));
+}
+;
+
+void ObjectInfoCache::getInfoBase(
+    const object_recognition_msgs::ObjectType & type,
+    object_recognition_core::prototypes::ObjectInfo &info) {
+
+  std::string object_hash = type.db + type.key;
+  std::string mesh_resource;
+
+  if (object_informations_.find(object_hash) != object_informations_.end()) {
+    info = object_informations_[object_hash];
+    return;
+  }
+
+  // Get the DB
+  object_recognition_core::db::ObjectDbParameters db_params(type.db);
+  std::string db_params_str;
+  or_json::mValue db_val = db_params.raw();
+  std::stringstream ss;
+  or_json::write(db_val, ss);
+  db_params_str = ss.str();
+  if (db_loaded_.find(db_params_str) == db_loaded_.end()) {
+    if (db_params.type()
+        == object_recognition_core::db::ObjectDbParameters::NONCORE) {
+      // If we're non-core, load the corresponding plugin
+      try {
+        db_loaded_[db_params_str] = db_class_loader_->createInstance(
+            db_params.raw().at("type").get_str());
+      } catch (pluginlib::PluginlibException& ex) {
+        //handle the class failing to load
+        ROS_ERROR("The plugin failed to load for some reason. Error: %s",
+                  ex.what());
+      }
+    } else {
+      db_loaded_[db_params_str] = db_params.generateDb();
+    }
+  }
+  object_recognition_core::db::ObjectDbPtr db = db_loaded_[db_params_str];
+
+  // Get information about the object
+  object_recognition_core::prototypes::ObjectInfo object_info;
+  try {
+    object_info = object_recognition_core::prototypes::ObjectInfo(type.key, db);
+  } catch (...) {
+    ROS_ERROR("Cannot retrieve load mesh Object db not initialized");
+  }
+  object_info.load_fields_and_attachments();
+}
+
+ObjectInfoDiskCache::~ObjectInfoDiskCache() {
+  // Clean up all the temporary files
+  for (std::map<std::string, std::string>::iterator iter = mesh_files_.begin();
+      iter != mesh_files_.end(); ++iter)
+    std::remove(iter->second.c_str());
+}
+
+void ObjectInfoDiskCache::getInfo(
+    const object_recognition_msgs::ObjectType & type,
+    object_recognition_core::prototypes::ObjectInfo &object_info) {
+
+  getInfoBase(type, object_info);
+
+  // Fill the mesh
+  std::string mesh_resource;
+  if (!(object_info.has_field("mesh_uri"))
+      && (object_info.has_attachment("mesh"))) {
+    // If the full mesh is stored in the object, save it to a temporary file and use it as the mesh URI
+    std::string file_name = std::string(std::tmpnam(0)) + ".stl";
+    std::ofstream file;
+    file.open(file_name.c_str(), std::ios::out | std::ios::binary);
+    std::stringstream stream;
+    object_info.get_attachment_stream("mesh", file);
+    file.close();
+    mesh_resource = std::string("file://") + file_name;
+    // Keep track of the files to delete them later
+    std::string object_hash = type.db + type.key;
+    mesh_files_[object_hash] = file_name;
+  }
+
+  object_info.set_field("mesh_uri", mesh_resource);
+}
+
+void ObjectInfoRamCache::getInfo(
+    const object_recognition_msgs::ObjectType & type,
+    object_recognition_msgs::ObjectInformation &info) {
+
+  object_recognition_core::prototypes::ObjectInfo object_info;
+  getInfoBase(type, object_info);
+
+  // Fill the name
+  if (object_info.has_field("name"))
+    info.name = object_info.get_field<std::string>("name");
+
+  // Read the mesh to the mesh member
+  std::string mesh_resource;
+  std::string mesh_file_name;
+  if (object_info.has_field("mesh_uri")) {
+    mesh_resource = object_info.get_field<std::string>("mesh_uri");
+  } else if (object_info.has_attachment("mesh")) {
+    // If the full mesh is stored in the object, save it to a temporary file and use it as the mesh URI
+    mesh_file_name = std::string(std::tmpnam(0)) + ".stl";
+    std::ofstream file(mesh_file_name.c_str(), std::ios::out | std::ios::binary);
+    object_info.get_attachment_stream("mesh", file);
+    file.close();
+    mesh_resource = std::string("file://") + mesh_file_name;
+  }
+
+  if (!mesh_resource.empty()) {
+    shapes::ShapeMsg shape_msg;
+    boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromResource(mesh_resource));
+    if (mesh)
+    {
+      shapes::constructMsgFromShape(mesh.get(), shape_msg);
+      info.ground_truth_mesh = boost::get<shape_msgs::Mesh>(shape_msg);
+    }
+    else
+      ROS_ERROR_STREAM("Unable to construct shape message from " << mesh_resource);
+  }
+  else
+    ROS_ERROR("Could not retrieve the mesh");
+
+  // Delete the temporary file
+  if (!mesh_file_name.empty())
+    std::remove(mesh_file_name.c_str());
+}
+}

src/info_service/CMakeLists.txt

@@ -1,5 +1,7 @@
 add_executable(object_information_server info_service.cpp)
-target_link_libraries(object_information_server ${catkin_LIBRARIES})
+target_link_libraries(object_information_server ${catkin_LIBRARIES}
+                                                object_information_cache
+)
 
 install(TARGETS object_information_server
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

src/info_service/info_service.cpp

@@ -35,7 +35,6 @@
 #include <fstream>
 #include <stdio.h>
 
-#include <geometric_shapes/shape_operations.h>
 #include <pluginlib/class_loader.h>
 #include <ros/ros.h>
 
@@ -45,77 +44,17 @@
 #include <object_recognition_core/db/prototypes/object_info.h>
 #include <object_recognition_msgs/GetObjectInformation.h>
 
+#include <object_recognition_ros/object_info_cache.h>
+
 static const std::string NODE_NAME = "object_info";
 
+static object_recognition_ros::ObjectInfoRamCache object_info_cache;
+
 static bool onServiceRequest(
     object_recognition_msgs::GetObjectInformation::Request &req,
     object_recognition_msgs::GetObjectInformation::Response &res) {
 
-  // Get the DB
-  object_recognition_core::db::ObjectDbPtr db;
-  object_recognition_core::db::ObjectDbParameters db_params(req.type.db);
-  if (db_params.type()
-      == object_recognition_core::db::ObjectDbParameters::NONCORE) {
-    // If we're non-core, load the corresponding plugin
-    try {
-      pluginlib::ClassLoader<object_recognition_core::db::ObjectDb> db_class_loader(
-          "object_recognition_core", "object_recognition_core::db::ObjectDb");
-      db = db_class_loader.createInstance(
-          db_params.raw().at("type").get_str());
-    } catch (pluginlib::PluginlibException& ex) {
-      //handle the class failing to load
-      ROS_ERROR("The plugin failed to load for some reason. Error: %s",
-                ex.what());
-    }
-  } else {
-    db = db_params.generateDb();
-  }
-
-  // Get information about the object
-  object_recognition_core::prototypes::ObjectInfo object_info;
-  try {
-    object_info = object_recognition_core::prototypes::ObjectInfo(
-        req.type.key, db);
-  } catch (...) {
-    ROS_ERROR("Cannot retrieve load mesh Object db not initialized");
-  }
-  object_info.load_fields_and_attachments();
-
-  // Fill the name
-  if (object_info.has_field("name"))
-    res.information.name = object_info.get_field<std::string>("name");
-
-  // Use the mesh information
-  std::string mesh_resource;
-  std::string mesh_file_name;
-  if (object_info.has_field("mesh_uri")) {
-    mesh_resource = object_info.get_field<std::string>("mesh_uri");
-  } else if (object_info.has_attachment("mesh")) {
-    // If the full mesh is stored in the object, save it to a temporary file and use it as the mesh URI
-    mesh_file_name = std::string(std::tmpnam(0)) + ".stl";
-    std::ofstream file(mesh_file_name.c_str(), std::ios::out | std::ios::binary);
-    object_info.get_attachment_stream("mesh", file);
-    file.close();
-    mesh_resource = std::string("file://") + mesh_file_name;
-  }
-
-  if (!mesh_resource.empty()) {
-    shapes::ShapeMsg shape_msg;
-    boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromResource(mesh_resource));
-    if (mesh)
-    {
-      shapes::constructMsgFromShape(mesh.get(), shape_msg);
-      res.information.ground_truth_mesh = boost::get<shape_msgs::Mesh>(shape_msg);
-    }
-    else
-      ROS_ERROR_STREAM("Unable to construct shape message from " << mesh_resource);
-  }
-  else
-    ROS_ERROR("Could not retrieve the mesh");
-
-  // Delete the temporary file
-  if (!mesh_file_name.empty())
-    std::remove(mesh_file_name.c_str());
+  object_info_cache.getInfo(req.type, res.information);
 
   return true;
 }