objects_database.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 // Author(s): Matei Ciocarlie
00036 
00037 #ifndef _OBJECTS_DATABASE_H_
00038 #define _OBJECTS_DATABASE_H_
00039 
00040 #include <boost/algorithm/string.hpp>
00041 #include <boost/foreach.hpp>
00042 #include <boost/lexical_cast.hpp>
00043 
00044 //for ROS error messages
00045 #include <ros/ros.h>
00046 
00047 #include <geometry_msgs/PoseStamped.h>
00048 
00049 #include <arm_navigation_msgs/Shape.h>
00050 
00051 #include <household_objects_database_msgs/DatabaseScan.h>
00052 
00053 #include <database_interface/postgresql_database.h>
00054 
00055 #include "household_objects_database/database_original_model.h"
00056 #include "household_objects_database/database_scaled_model.h"
00057 #include "household_objects_database/database_grasp.h"
00058 #include "household_objects_database/database_mesh.h"
00059 #include "household_objects_database/database_perturbation.h"
00060 #include "household_objects_database/database_scan.h"
00061 #include "household_objects_database/database_view.h"
00062 #include "household_objects_database/database_vfh.h"
00063 #include "household_objects_database/database_vfh_orientation.h"
00064 #include "household_objects_database/database_file_path.h"
00065 #include "household_objects_database/database_task.h"
00066 #include "household_objects_database/database_capture_region.h"
00067 #include "household_objects_database/database_object_paths.h"
00068 
00069 namespace household_objects_database
00070 {
00071 
00072   class DatabaseTask;
00073 
00075   class ObjectsDatabase : public database_interface::PostgresqlDatabase
00076   {
00077   public:
00079     ObjectsDatabase (std::string host, std::string port, std::string user, std::string password, std::string dbname) :
00080       PostgresqlDatabase (host, port, user, password, dbname)
00081     {
00082     }
00083 
00085     ObjectsDatabase (const database_interface::PostgresqlDatabaseConfig &config) :
00086       PostgresqlDatabase (config)
00087     {
00088     }
00089 
00091     ~ObjectsDatabase ()
00092     {
00093     }
00094 
00096 
00099     virtual bool
00100     acquireNextTask (std::vector<boost::shared_ptr<DatabaseTask> > &task, std::vector<std::string> accepted_types);
00101 
00102     //------- helper functions wrapped around the general versions for convenience -------
00103     //----------------- or for cases where where_clauses are needed ----------------------
00104 
00106     bool
00107     getOriginalModelsList (std::vector<boost::shared_ptr<DatabaseOriginalModel> > &models) const
00108     {
00109       DatabaseOriginalModel example;
00110       return getList<DatabaseOriginalModel> (models, example, "");
00111     }
00112 
00114     bool
00115     getScaledModelsList (std::vector<boost::shared_ptr<DatabaseScaledModel> > &models) const
00116     {
00117       DatabaseScaledModel example;
00118       return getList<DatabaseScaledModel> (models, example, "");
00119     }
00120 
00122     bool
00123     getScaledModelsByAcquisition (std::vector<boost::shared_ptr<DatabaseScaledModel> > &models,
00124                                   std::string acquisition_method) const
00125     {
00126       std::string where_clause ("acquisition_method_name='" + acquisition_method + "'");
00127       DatabaseScaledModel example;
00128       //this should be set by default, but let's make sure again
00129       example.acquisition_method_.setReadFromDatabase (true);
00130       return getList<DatabaseScaledModel> (models, example, where_clause);
00131     }
00132 
00133     virtual bool
00134     getScaledModelsBySet (std::vector<boost::shared_ptr<DatabaseScaledModel> > &models, 
00135                           std::string model_set_name) const
00136     {
00137       if (model_set_name.empty ())
00138         return getScaledModelsList (models);
00139       std::string where_clause = std::string ("original_model_id IN (SELECT original_model_id FROM "
00140         "model_set WHERE model_set_name = '") + model_set_name + std::string ("')");
00141       DatabaseScaledModel example;
00142       return getList<DatabaseScaledModel> (models, example, where_clause);
00143     }
00144 
00146     bool
00147     getNumOriginalModels (int &num_models) const
00148     {
00149       DatabaseOriginalModel example;
00150       return countList (&example, num_models, "");
00151     }
00152 
00153     bool
00154     getScaledModelsByRecognitionId(std::vector<boost::shared_ptr<DatabaseScaledModel> > &models, 
00155                                    std::string recognition_id) const
00156     {
00157       std::string where_clause = 
00158         std::string("original_model_id IN (SELECT original_model_id FROM "
00159                     "original_model WHERE original_model_recognition_id = '")
00160         + recognition_id + std::string("')");
00161       DatabaseScaledModel example;
00162       return getList<DatabaseScaledModel> (models, example, where_clause);
00163     }
00164 
00166     bool
00167     getModelRoot (std::string& root) const
00168     {
00169       return getVariable ("'MODEL_ROOT'", root);
00170     }
00171 
00173     bool
00174     getModelsListByTags (std::vector<boost::shared_ptr<DatabaseOriginalModel> > &models, 
00175                          std::vector<std::string> tags) const
00176     {
00177       DatabaseOriginalModel example;
00178       std::string where_clause ("(");
00179       for (size_t i = 0; i < tags.size (); i++)
00180       {
00181         if (i != 0)
00182           where_clause += "AND ";
00183         where_clause += "'" + tags[i] + "' = ANY (original_model_tags)";
00184       }
00185       where_clause += ")";
00186       return getList<DatabaseOriginalModel> (models, example, where_clause);
00187     }
00188 
00190     bool
00191     getGrasps (int scaled_model_id, std::string hand_name, std::vector<boost::shared_ptr<DatabaseGrasp> > &grasps) const
00192     {
00193       DatabaseGrasp example;
00194       std::stringstream id;
00195       id << scaled_model_id;
00196       std::string where_clause ("scaled_model_id=" + id.str () + " AND hand_name='" + hand_name + "'");
00197       //+ " AND grasp_energy >= 0 AND grasp_energy <= 10" );
00198       return getList<DatabaseGrasp> (grasps, example, where_clause);
00199     }
00200 
00202     bool
00203     getClusterRepGrasps (int scaled_model_id, std::string hand_name,
00204                          std::vector<boost::shared_ptr<DatabaseGrasp> > &grasps) const
00205     {
00206       DatabaseGrasp example;
00207       std::stringstream id;
00208       id << scaled_model_id;
00209       std::string where_clause ("scaled_model_id=" + id.str () + " AND hand_name='" + hand_name + "'"
00210           + " AND grasp_cluster_rep=true AND grasp_energy >= 0 AND grasp_energy <= 10");
00211       return getList<DatabaseGrasp> (grasps, example, where_clause);
00212     }
00213 
00215     bool
00216     getScaledModelMesh (int scaled_model_id, DatabaseMesh &mesh) const
00217     {
00218       //first get the original model id
00219       DatabaseScaledModel scaled_model;
00220       scaled_model.id_.data () = scaled_model_id;
00221       if (!loadFromDatabase (&scaled_model.original_model_id_))
00222       {
00223         ROS_ERROR ("Failed to get original model for scaled model id %d", scaled_model_id);
00224         return false;
00225       }
00226       mesh.id_.data () = scaled_model.original_model_id_.data ();
00227       if (!loadFromDatabase (&mesh.triangles_) || !loadFromDatabase (&mesh.vertices_))
00228       {
00229         ROS_ERROR ("Failed to load mesh from database for scaled model %d, resolved to original model %d",
00230                    scaled_model_id, mesh.id_.data ());
00231         return false;
00232       }
00233       return true;
00234     }
00235 
00237     bool
00238     getScaledModelMesh (int scaled_model_id, arm_navigation_msgs::Shape &shape) const
00239     {
00240       DatabaseMesh mesh;
00241       if (!getScaledModelMesh (scaled_model_id, mesh))
00242         return false;
00243       shape.triangles = mesh.triangles_.data ();
00244       shape.vertices.clear ();
00245       if (mesh.vertices_.data ().size () % 3 != 0)
00246       {
00247         ROS_ERROR ("Get scaled model mesh: size of vertices vector is not a multiple of 3");
00248         return false;
00249       }
00250       for (size_t i = 0; i < mesh.vertices_.data ().size () / 3; i++)
00251       {
00252         geometry_msgs::Point p;
00253         p.x = mesh.vertices_.data ().at (3 * i + 0);
00254         p.y = mesh.vertices_.data ().at (3 * i + 1);
00255         p.z = mesh.vertices_.data ().at (3 * i + 2);
00256         shape.vertices.push_back (p);
00257       }
00258       shape.type = shape.MESH;
00259       return true;
00260     }
00261 
00262     bool
00263     getModelScans (int scaled_model_id, std::string source,
00264                    std::vector<household_objects_database_msgs::DatabaseScan> &matching_scan_list) const
00265     {
00266       DatabaseScan scan;
00267       std::vector<boost::shared_ptr<DatabaseScan> > matching_scans;
00268       std::string where_clause ("scaled_model_id = " + boost::lexical_cast<std::string> (scaled_model_id) + " AND "
00269           + "scan_source = '" + source + "'");
00270       getList<DatabaseScan> (matching_scans, where_clause);
00271 
00272       BOOST_FOREACH(const boost::shared_ptr<DatabaseScan> &scan, matching_scans)
00273             {
00274               household_objects_database_msgs::DatabaseScan out_scan;
00275               out_scan.model_id = scan->scaled_model_id_.get ();
00276               out_scan.scan_source = scan->scan_source_.get ();
00277               out_scan.bagfile_location = scan->scan_bagfile_location_.get ();
00278 
00279               out_scan.pose.pose = scan->object_pose_.get ().pose_;
00280               out_scan.pose.header.frame_id = scan->frame_id_.get ();
00281               out_scan.cloud_topic = scan->cloud_topic_.get ();
00282               matching_scan_list.push_back (out_scan);
00283             }
00284 
00285       return true;
00286     }
00287 
00292 
00293     bool
00294     getAllPerturbationsForModel (int scaled_model_id, std::vector<DatabasePerturbationPtr> &perturbations)
00295     {
00296       std::string where_clause = std::string ("grasp_id = ANY(ARRAY(SELECT grasp_id FROM grasp WHERE "
00297         "scaled_model_id = " + boost::lexical_cast<std::string> (scaled_model_id) + std::string ("))"));
00298       DatabasePerturbation example;
00299       return getList<DatabasePerturbation> (perturbations, example, where_clause);
00300     }
00301 
00302     bool
00303     getPerturbationsForGrasps (const std::vector<int> &grasp_ids, std::vector<DatabasePerturbationPtr> &perturbations)
00304     {
00305       std::vector<std::string> grasp_id_strs;
00306       grasp_id_strs.reserve (grasp_ids.size ());
00307       BOOST_FOREACH(int id, grasp_ids)
00308             {
00309               grasp_id_strs.push_back (boost::lexical_cast<std::string, int> (id));
00310             }
00311       std::string where_clause = std::string ("grasp_id = ANY(ARRAY[" + boost::algorithm::join (grasp_id_strs, ", ")
00312           + "])");
00313       DatabasePerturbation example;
00314       return getList<DatabasePerturbation> (perturbations, example, where_clause);
00315     }
00316 
00317     bool
00318     getVFHDescriptors (std::vector<boost::shared_ptr<DatabaseVFH> > &vfh)
00319     {
00320       DatabaseVFH example;
00321       if (!getList<DatabaseVFH> (vfh, example, ""))
00322         return false;
00323       for (size_t i = 0; i < vfh.size (); i++)
00324       {
00325         if (!loadFromDatabase (&(vfh[i]->vfh_descriptor_)))
00326         {
00327           ROS_ERROR ("Failed to load descriptor data for vfh id %d", vfh[i]->vfh_id_.data ());
00328         }
00329       }
00330     }
00331 
00332     bool
00333    getViewFromViewIdNoData (int view_id, boost::shared_ptr<DatabaseView> &view)
00334    {
00335      std::vector<boost::shared_ptr<DatabaseView> > views;
00336      std::stringstream where2;
00337      where2 << "view_id =" << view_id;
00338      std::string where_clause2 (where2.str ());
00339      getList (views, where_clause2);
00340      view = views[0];
00341      return true;
00342    }
00343 
00344     bool
00345     getViewFromVFHId (int vfh_id, boost::shared_ptr<DatabaseView> &view)
00346     {
00347       std::vector<boost::shared_ptr<DatabaseVFH> > vfhs;
00348       std::stringstream where;
00349       where << "vfh_id =" << vfh_id;
00350       std::string where_clause (where.str ());
00351       if (!getList (vfhs, where_clause)) {
00352         return false;
00353       }
00354 
00355       std::vector<boost::shared_ptr<DatabaseView> > views;
00356       std::stringstream where2;
00357       where2 << "view_id =" << vfhs[0]->view_id_.data ();
00358       std::string where_clause2 (where2.str ());
00359       getList (views, where_clause2);
00360       if (!loadFromDatabase (&views[0]->view_point_cloud_data_)) {
00361         ROS_ERROR ("Failed to load view point cloud data for view id %d", vfhs[0]->view_id_.data ());
00362       }
00363       view = views[0];
00364       return true;
00365     }
00366 
00367     bool
00368     getViewFromVFHIdNoData (int vfh_id, boost::shared_ptr<DatabaseView> &view)
00369     {
00370       std::vector<boost::shared_ptr<DatabaseVFH> > vfhs;
00371       std::stringstream where;
00372       where << "vfh_id =" << vfh_id;
00373       std::string where_clause (where.str ());
00374       if (!getList (vfhs, where_clause)) {
00375         return false;
00376       }
00377 
00378       std::vector<boost::shared_ptr<DatabaseView> > views;
00379       std::stringstream where2;
00380       where2 << "view_id =" << vfhs[0]->view_id_.data ();
00381       std::string where_clause2 (where2.str ());
00382       getList (views, where_clause2);
00383       view = views[0];
00384       return true;
00385     }
00386 
00387     bool
00388     getVFHFromView (boost::shared_ptr<DatabaseVFH> & vfh, 
00389                     boost::shared_ptr<household_objects_database::DatabaseView> &view)
00390     {
00391       std::vector<boost::shared_ptr<DatabaseVFH> > vfhs;
00392       std::stringstream where;
00393       where << "view_id =" << view->view_id_.data();
00394       std::string where_clause (where.str ());
00395       if (!getList (vfhs, where_clause)) {
00396         return false;
00397       }
00398 
00399       vfh = vfhs[0];
00400       if (!loadFromDatabase (&vfh->vfh_descriptor_)) {
00401         ROS_ERROR ("Failed to load VFH descriptor for %d", vfh->vfh_id_.data ());
00402       }
00403       return true;
00404     }
00405 
00406     bool
00407     getOrientationRollFromVFHId (int vfh_id, boost::shared_ptr<DatabaseVFHOrientation> &roll_histogram)
00408     {
00409       std::vector<boost::shared_ptr<DatabaseVFH> > vfhs;
00410       std::stringstream where;
00411       where << "vfh_id =" << vfh_id;
00412       std::string where_clause (where.str ());
00413       if (!getList (vfhs, where_clause)) {
00414         return false;
00415       }
00416 
00417       /*std::vector<boost::shared_ptr<DatabaseView> > views;
00418       std::stringstream where2;
00419       where2 << "view_id =" << vfhs[0]->view_id_.data ();
00420       std::string where_clause2 (where2.str ());
00421       getList (views, where_clause2);*/
00422 
00423       //Once we have the view, load vfh orientation
00424       std::vector<boost::shared_ptr<DatabaseVFHOrientation> > rolls;
00425       std::stringstream where3;
00426       where3 << "vfh_id =" << vfhs[0]->vfh_id_.data ();
00427       std::string where_clause3 (where3.str ());
00428       if (!getList (rolls, where_clause3)) {
00429         return false;
00430       }
00431 
00432       //std::cout << "rolls size:" << rolls.size() << std::endl;
00433       if (rolls.size() == 0) {
00434         return false;
00435       }
00436 
00437       if (!loadFromDatabase (&rolls[0]->vfh_orientation_descriptor_)) {
00438         ROS_ERROR ("Failed to load VFH roll orientation histogram => id %d", rolls[0]->vfh_orientation_id_.data ());
00439       }
00440       roll_histogram = rolls[0];
00441       return true;
00442     }
00443 
00444     bool
00445     getOrientationRollFromVFHThroughViewId (int vfh_id, boost::shared_ptr<DatabaseVFHOrientation> &roll_histogram)
00446     {
00447       std::vector<boost::shared_ptr<DatabaseVFH> > vfhs;
00448       std::stringstream where;
00449       where << "vfh_id =" << vfh_id;
00450       std::string where_clause (where.str ());
00451       if (!getList (vfhs, where_clause)) {
00452         return false;
00453       }
00454 
00455       std::vector<boost::shared_ptr<DatabaseView> > views;
00456       std::stringstream where2;
00457       where2 << "view_id =" << vfhs[0]->view_id_.data ();
00458       std::string where_clause2 (where2.str ());
00459       getList (views, where_clause2);
00460 
00461       //Once we have the view, load vfh orientation
00462       std::vector<boost::shared_ptr<DatabaseVFHOrientation> > rolls;
00463       std::stringstream where3;
00464       where3 << "view_id =" << views[0]->view_id_.data ();
00465       std::string where_clause3 (where3.str ());
00466       getList (rolls, where_clause3);
00467 
00468       if (!loadFromDatabase (&rolls[0]->vfh_orientation_descriptor_)) {
00469         ROS_ERROR ("Failed to load VFH roll orientation histogram => id %d", rolls[0]->vfh_orientation_id_.data ());
00470       }
00471       roll_histogram = rolls[0];
00472       return true;
00473     }
00474 
00475     bool
00476     getModelScaleFromId (int scaled_model_id, boost::shared_ptr<DatabaseScaledModel> &scale_model)
00477     {
00478       std::vector<boost::shared_ptr<DatabaseScaledModel> > scaled_models;
00479       std::stringstream where;
00480       where << "scaled_model_id =" << scaled_model_id;
00481       std::string where_clause (where.str ());
00482       if (!getList (scaled_models, where_clause))
00483         return false;
00484 
00485       scale_model = scaled_models[0];
00486       return true;
00487     }
00488 
00489     bool
00490     getModelPathFromViewId (boost::shared_ptr<DatabaseView> &view, std::string &path)
00491     {
00492 
00493       boost::shared_ptr<DatabaseScaledModel> scaled_model;
00494       getModelScaleFromId (view->scaled_model_id_.data (), scaled_model);
00495       //get path...
00496       std::vector<boost::shared_ptr<DatabaseFilePath> > file_paths;
00497       std::stringstream where3;
00498       where3 << "original_model_id =" << scaled_model->original_model_id_.data ();
00499       std::string where_clause3 (where3.str ());
00500       if (!getList (file_paths, where_clause3))
00501         return false;
00502 
00503       path = file_paths[0]->file_path_.data ();
00504       return true;
00505     }
00506 
00507     bool
00508     getModelScale (boost::shared_ptr<DatabaseView> &view, double * scale_factor)
00509     {
00510       boost::shared_ptr<DatabaseScaledModel> scaled_model;
00511       getModelScaleFromId (view->scaled_model_id_.data (), scaled_model);
00512       *scale_factor = scaled_model->scale_.data ();
00513       return true;
00514     }
00515 
00516     bool
00517     getViewsFromScaledModelId (int scaled_model_id, int iteration, std::vector<boost::shared_ptr<DatabaseView> > & views)
00518     {
00519       std::stringstream where;
00520       where << "scaled_model_id =" << scaled_model_id << " AND iteration =" << iteration;
00521       std::string where_clause (where.str ());
00522       if (!getList (views, where_clause)) {
00523         return false;
00524       }
00525 
00526       return true;
00527     }
00528 
00530     bool
00531     getCaptureRegions (int scaled_model_id, std::string robot_geometry_hash, std::vector<boost::shared_ptr<DatabaseCaptureRegion> > &capture_regions) const
00532     {
00533       DatabaseCaptureRegion example;
00534       std::stringstream id;
00535       id << scaled_model_id;
00536       std::string where_clause ("scaled_model_id=" + id.str () + " AND robot_geometry_hash='" + robot_geometry_hash + "'" );
00537       return getList<DatabaseCaptureRegion> (capture_regions, example, where_clause);
00538     }
00539 
00541     bool
00542     getObjectPaths (int scaled_model_id, std::string robot_geometry_hash, std::vector<boost::shared_ptr<DatabaseObjectPaths> > &object_paths) const
00543     {
00544       DatabaseObjectPaths example;
00545       std::stringstream id;
00546       id << scaled_model_id;
00547       std::string where_clause ("object_db_id=" + id.str () + " AND robot_geometry_hash='" + robot_geometry_hash + "'" );
00548       return getList<DatabaseObjectPaths> (object_paths, example, where_clause);
00549     }
00550 
00551 
00552   };
00553   typedef boost::shared_ptr<ObjectsDatabase> ObjectsDatabasePtr;
00554 }//namespace
00555 
00556 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


household_objects_database
Author(s): Matei Ciocarlie, except for source files individually marked otherwise
autogenerated on Fri Jan 25 2013 15:02:09