get the service to fill the mesh information

CMakeLists.txt

@@ -4,8 +4,8 @@ project(object_recognition_ros)
 find_package(catkin REQUIRED)
 
 if (${catkin_VERSION} VERSION_GREATER "0.5.28")
-find_package(catkin REQUIRED ecto ecto_ros geometry_msgs object_recognition_core
-                             object_recognition_msgs rosbag roscpp rviz sensor_msgs tf
+find_package(catkin REQUIRED ecto ecto_ros geometric_shapes geometry_msgs object_recognition_core
+                             object_recognition_msgs pluginlib rosbag roscpp rviz sensor_msgs tf
                              visualization_msgs
 )
 catkin_package()

package.xml

@@ -15,6 +15,7 @@
   <build_depend>ecto</build_depend>
   <build_depend>ecto_ros</build_depend>
   <build_depend>ecto_image_pipeline</build_depend>
+  <build_depend>geometric_shapes</build_depend>
   <build_depend>object_recognition_core</build_depend>
   <build_depend>object_recognition_msgs</build_depend>
   <build_depend>pcl</build_depend>
@@ -27,6 +28,7 @@
   <run_depend>ecto</run_depend>
   <run_depend>ecto_ros</run_depend>
   <run_depend>ecto_image_pipeline</run_depend>
+  <run_depend>geometric_shapes</run_depend>
   <run_depend>object_recognition_msgs</run_depend>
   <run_depend>pcl</run_depend>
   <run_depend>pluginlib</run_depend>

src/CMakeLists.txt

@@ -1,6 +1,9 @@
-add_subdirectory(io)
-add_subdirectory(rviz_plugin)
+if (${ROS_GROOVY_OR_ABOVE_FOUND})
 add_subdirectory(info_service)
+add_subdirectory(rviz_plugin)
+endif()
+
+add_subdirectory(io)
 
 # generate ecto cells to wrap the different messages
 pubsub_gen_wrap(object_recognition_msgs DESTINATION object_recognition_ros/ecto_cells

src/info_service/info_service.cpp

@@ -32,28 +32,98 @@
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
+#include <fstream>
+#include <stdio.h>
+
+#include <geometric_shapes/shape_operations.h>
+#include <pluginlib/class_loader.h>
 #include <ros/ros.h>
-#include <object_recognition_mgs/GetObjectInformation.h>
+
+#include <object_recognition_core/db/db.h>
+#include <object_recognition_core/db/db_base.h>
+#include <object_recognition_core/db/db_parameters.h>
+#include <object_recognition_core/db/prototypes/object_info.h>
+#include <object_recognition_msgs/GetObjectInformation.h>
 
 static const std::string NODE_NAME = "object_info";
 
-static bool onServiceRequest(object_recognition_mgs::GetObjectInformation::Request &req,
-                             object_recognition_mgs::GetObjectInformation::Response &res)
-{
-
-  return true;  
+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;
+    file.open(mesh_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://") + mesh_file_name;
+  }
+
+  if (!mesh_resource.empty()) {
+    shapes::ShapeMsg shape_msg;
+    shapes::constructMsgFromShape(shapes::createMeshFromResource(mesh_resource), shape_msg);
+    res.information.ground_truth_mesh = boost::get<shape_msgs::Mesh>(shape_msg);
+  }
+  // Delete the temporary file
+  if (!mesh_file_name.empty())
+    std::remove(mesh_file_name.c_str());
+
+
+  return true;
 }
 
-int main(int argc, char **argv)
-{
+int main(int argc, char **argv) {
   ros::init(argc, argv, NODE_NAME);
-  
+
   ros::AsyncSpinner spinner(1);
   spinner.start();
-  
+
   ros::NodeHandle nh;
-  ros::ServiceServer info_service = nh.advertiseService("get_object_info", &onServiceRequest);
-  ros::waitForShutdown();   
-
+  ros::ServiceServer info_service = nh.advertiseService("get_object_info",
+                                                        &onServiceRequest);
+  ros::waitForShutdown();
+
   return 0;
 }

src/rviz_plugin/CMakeLists.txt

@@ -1,7 +1,3 @@
-if (${ROS_FUERTE_FOUND})
-return()
-endif()
-
 include_directories(${catkin_INCLUDE_DIRS})
 
 find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED)