graspit: ros_database_manager.cpp Source File

00001 //######################################################################
00002 //
00003 // GraspIt!
00004 // Copyright (C) 2002-2009  Columbia University in the City of New York.
00005 // All rights reserved.
00006 //
00007 // GraspIt! is free software: you can redistribute it and/or modify
00008 // it under the terms of the GNU General Public License as published by
00009 // the Free Software Foundation, either version 3 of the License, or
00010 // (at your option) any later version.
00011 //
00012 // GraspIt! is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 // GNU General Public License for more details.
00016 //
00017 // You should have received a copy of the GNU General Public License
00018 // along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
00019 //
00020 // Author(s):  Matei T. Ciocarlie
00021 //
00022 // $Id: ros_database_manager.cpp,v 1.9 2010/09/01 23:54:20 cmatei Exp $
00023 //
00024 //######################################################################
00025 
00026 #include "ros_database_manager.h"
00027 
00028 #include "household_objects_database/objects_database.h"
00029 #include "household_objects_database/database_scaled_model.h"
00030 #include "household_objects_database/database_grasp.h"
00031 #include "household_objects_database/database_task.h"
00032 #include "household_objects_database/database_file_path.h"
00033 
00034 #include <boost/shared_ptr.hpp>
00035 #include <sstream>
00036 
00037 //so that overloaded operators work...
00038 using namespace database_interface;
00039 
00040 namespace db_planner {
00041 
00042 RosDatabaseManager::RosDatabaseManager(std::string host, std::string port, std::string user,
00043                                        std::string password, std::string dbname,
00044                                        ModelAllocator *model_allocator, GraspAllocator* grasp_allocator)
00045 {
00046   model_allocator_ = model_allocator;
00047   grasp_allocator_ = grasp_allocator;
00048   database_ = new household_objects_database::ObjectsDatabase(host, port, user, password, dbname);
00049 }
00050 
00051 RosDatabaseManager::~RosDatabaseManager()
00052 {
00053   delete model_allocator_;
00054   delete grasp_allocator_;
00055   delete database_;
00056 }
00057 
00058 
00059 bool RosDatabaseManager::isConnected() const
00060 {
00061   return database_->isConnected();
00062 }
00063 
00064 void RosDatabaseManager::modelFromDBModel(Model *model, 
00065                                           const household_objects_database::DatabaseScaledModel &db_model, 
00066                                           std::string model_root) const
00067 {
00068    model->SetModelId( db_model.id_.get() );
00069    model->SetScale( db_model.scale_.get() );
00070    model->SetRescaleFactor( model->Scale() );
00071    std::ostringstream scalestr;
00072    scalestr << model->Scale();
00073    std::ostringstream idstr;
00074    idstr << db_model.original_model_id_.data();
00075    model->SetModelName( idstr.str() + "_" + db_model.model_.data() + "_" + scalestr.str());
00076    model->SetTags( db_model.tags_.data().begin(), db_model.tags_.data().end() );
00077 
00078    //see what kind of files we have in the database for this model
00079    std::vector< boost::shared_ptr<household_objects_database::DatabaseFilePath> > paths;
00080    std::stringstream id;
00081    id << db_model.original_model_id_.data();
00082    std::string where_clause( "original_model_id=" + id.str());
00083    if (!database_->getList<household_objects_database::DatabaseFilePath>(paths, where_clause)) {
00084      std::cerr << "Failed to load file paths from database\n";
00085      return;
00086    }
00087    for (size_t i=0; i<paths.size(); i++) {
00088      if (paths[i]->file_type_.data() == "THUMBNAIL_BINARY_PNG") {
00089        model->SetThumbnailPath(model_root + paths[i]->file_path_.data());
00090      } else if (paths[i]->file_type_.data() == "GEOMETRY_BINARY_PLY") {
00091        model->SetGeometryPath(model_root + paths[i]->file_path_.data());
00092      }
00093    }
00094 }
00095 
00096 bool RosDatabaseManager::ModelList(vector<Model*>* model_list, FilterList::FilterType filter) const
00097 {
00098   std::string model_root;
00099   if (!database_->getModelRoot(model_root)) return false;
00100   std::vector< boost::shared_ptr<household_objects_database::DatabaseScaledModel>  >db_model_list;
00101   if (!database_->getScaledModelsList(db_model_list)) return false;
00102   for (size_t i=0; i<db_model_list.size(); i++)
00103   {
00104     Model *model = model_allocator_->Get();
00105     modelFromDBModel(model, *(db_model_list[i]), model_root);
00106     model_list->push_back(model);
00107   }
00108   return true;
00109 }
00110 
00111 bool RosDatabaseManager::ScaledModel(Model* &model, int scaled_model_id) const
00112 {
00113   std::string model_root;
00114   if (!database_->getModelRoot(model_root)) return false;
00115   std::stringstream id;
00116   id << scaled_model_id;
00117   std::string where_clause("scaled_model_id=" + id.str());
00118   std::vector< boost::shared_ptr<household_objects_database::DatabaseScaledModel>  >db_model_list;
00119   if (!database_->getList<household_objects_database::DatabaseScaledModel>(db_model_list, where_clause)) return false;
00120   if (db_model_list.size() != 1) return false;
00121   model = model_allocator_->Get();
00122   modelFromDBModel(model, *(db_model_list[0]), model_root);
00123   return true;
00124 }
00125 
00126 bool RosDatabaseManager::AcquireNextTask(TaskRecord *rec, std::vector<std::string> accepted_types)
00127 {
00128   std::vector< boost::shared_ptr<household_objects_database::DatabaseTask> > db_task;
00129   if (!database_->acquireNextTask(db_task, accepted_types)) return false;
00130   if (db_task.empty()) 
00131   {
00132     //no task to be run
00133     rec->taskType = "";
00134     return true;    
00135   }
00136   //populate the entry
00137   rec->taskType = db_task[0]->type_.data();
00138   rec->taskId = db_task[0]->id_.data();
00139   return true;
00140 }
00141 
00142 bool RosDatabaseManager::SetTaskStatus(int task_id, const string &status)
00143 {
00144   household_objects_database::DatabaseTask db_task;
00145   db_task.id_.data() = task_id;
00146   db_task.outcome_name_.data() = status;
00147   if ( !database_->saveToDatabase(&(db_task.outcome_name_)) ) return false;
00148   return true;
00149 }
00150 
00151 bool RosDatabaseManager::GetPlanningTaskRecord(int task_id, PlanningTaskRecord *rec)
00152 {
00153   household_objects_database::DatabasePlanningTask planningTask;
00154   planningTask.id_.data() = task_id;
00155   if ( !database_->loadFromDatabase(&(planningTask.scaled_model_id_)) ||
00156        !database_->loadFromDatabase(&(planningTask.hand_name_)) ||
00157        !database_->loadFromDatabase(&(planningTask.time_)) )
00158   {
00159     std::cout << "Failed to load planning task for task id " << task_id << "\n";
00160     return false;
00161   }
00162   rec->taskId = task_id;
00163   rec->taskTime = planningTask.time_.data();
00164   rec->handName = planningTask.hand_name_.data();
00165   //load the actual model
00166   //a fairly hacky way to do it for now, field-by-field
00167   household_objects_database::DatabaseScaledModel db_model;
00168   db_model.id_.data() = planningTask.scaled_model_id_.data();
00169   //first we need to read the original model id so we can then read the rest
00170   if ( !database_->loadFromDatabase(&(db_model.original_model_id_)) ) return false;
00171   if ( !database_->loadFromDatabase(&(db_model.scale_)) ||
00172        !database_->loadFromDatabase(&(db_model.tags_)) )
00173   {
00174     return false;
00175   }
00176   Model *model = model_allocator_->Get();
00177   std::string model_root;
00178   if (!database_->getModelRoot(model_root)) return false;
00179   modelFromDBModel(model, db_model, model_root);
00180   rec->model = model;
00181   return true;
00182 }
00183 
00184 bool RosDatabaseManager::GetOptimizationTaskRecord(int task_id, OptimizationTaskRecord *rec)
00185 {
00186   household_objects_database::DatabaseOptimizationTask optTask;
00187   optTask.dbase_task_id_.data() = task_id;
00188   if ( !database_->loadFromDatabase(&(optTask.parameters_)) || 
00189        !database_->loadFromDatabase(&(optTask.hand_name_)) )
00190   {
00191     std::cout << "Failed to load optimization task for task id " << task_id << "\n";
00192     return false;
00193   }
00194   rec->taskId = task_id;
00195   rec->parameters = optTask.parameters_.data();
00196   rec->hand_name = optTask.hand_name_.data();
00197   return true;
00198 }
00199 
00200 bool RosDatabaseManager::SaveOptimizationResults(const OptimizationTaskRecord &rec,
00201                                                  const std::vector<double>& parameters,
00202                                                  const std::vector<double>& results,
00203                                                  const std::vector<double>& seed )
00204 {
00205   household_objects_database::DatabaseOptimizationResult optResult;
00206   optResult.dbase_task_id_.data() = rec.taskId;
00207   optResult.parameters_.data() = parameters;
00208   optResult.hand_name_.data() = rec.hand_name;
00209   optResult.results_.data() = results;
00210   optResult.seed_.data() = seed;
00211   
00212   if (!database_->insertIntoDatabase(&optResult))
00213   {
00214     std::cout << "Failed to save optimization results for task id " << rec.taskId << "\n";
00215     return false;
00216   }
00217   return true;
00218 }
00219 
00220 bool RosDatabaseManager::GraspTypeList(vector<string>* type_list) const
00221 {
00222   type_list->push_back("ALL");
00223   return true;
00224 }
00225 
00226 bool RosDatabaseManager::GetGrasps(const Model& model, const string& hand_name, 
00227                                    vector<Grasp*>* grasp_list) const
00228 {
00229   std::vector< boost::shared_ptr<household_objects_database::DatabaseGrasp> > db_grasp_list;
00230   if ( !database_->getGrasps(model.ModelId(), hand_name, db_grasp_list) ) return false;
00231   for (size_t i=0; i<db_grasp_list.size(); i++)
00232   {
00233     Grasp *grasp = grasp_allocator_->Get();
00234     const household_objects_database::DatabaseGrasp &db_grasp = *(db_grasp_list[i]);
00235 
00236     grasp->SetSourceModel(model);
00237     grasp->SetHandName(hand_name);
00238 
00239     grasp->SetGraspId(db_grasp.id_.get());
00240     grasp->SetEnergy(db_grasp.quality_.get());
00241     grasp->SetEpsilonQuality(0.0);
00242     grasp->SetVolumeQuality(0.0);
00243     grasp->SetClearance(db_grasp.pre_grasp_clearance_.get());
00244     grasp->SetClusterRep(db_grasp.cluster_rep_.get());
00245     grasp->SetHandName(db_grasp.hand_name_.get());
00246     grasp->SetTableClearance(db_grasp.table_clearance_.get());
00247     grasp->SetCompliantCopy(db_grasp.compliant_copy_.get());
00248     grasp->SetCompliantOriginalId(db_grasp.compliant_original_id_.get());
00249     
00250     std::stringstream str;
00251     std::vector<double> pregrasp_joints;
00252     str << db_grasp.pre_grasp_posture_.get(); 
00253     str >> pregrasp_joints;
00254     if (str.fail()) return false;
00255     std::vector<double> pregrasp_position;
00256     str << db_grasp.pre_grasp_pose_.get();
00257     str >> pregrasp_position;
00258     if (str.fail()) return false;
00259     std::vector<double> grasp_joints;
00260     str << db_grasp.final_grasp_posture_.get(); 
00261     str >> grasp_joints; 
00262     if (str.fail()) return false;
00263     std::vector<double> grasp_position;
00264     str << db_grasp.final_grasp_pose_.get();
00265     str >> grasp_position;
00266     if (str.fail()) return false;
00267     
00268     //convert from ROS units to Graspit units
00269     pregrasp_position[0] *= 1000;
00270     pregrasp_position[1] *= 1000;
00271     pregrasp_position[2] *= 1000;
00272     grasp_position[0] *= 1000;
00273     grasp_position[1] *= 1000;
00274     grasp_position[2] *= 1000;
00275 
00276     grasp->SetGraspParameters(pregrasp_joints, 
00277                              pregrasp_position, 
00278                              grasp_joints, 
00279                              grasp_position);
00280     grasp->SetPregraspJoints(pregrasp_joints);
00281     grasp->SetPregraspPosition(pregrasp_position);
00282     grasp->SetFinalgraspJoints(grasp_joints);
00283     grasp->SetFinalgraspPosition(grasp_position);
00284     
00285     grasp_list->push_back(grasp);
00286   }
00287   return true;
00288 }
00289 
00290 bool RosDatabaseManager::SaveGrasp(const Grasp* grasp) const
00291 {
00292   household_objects_database::DatabaseGrasp db_grasp;
00293   db_grasp.scaled_model_id_.get() = grasp->SourceModel().ModelId();
00294   db_grasp.quality_.get() = grasp->Energy();
00295   db_grasp.pre_grasp_clearance_.get() = grasp->Clearance();
00296   db_grasp.cluster_rep_.get() = grasp->ClusterRep();
00297   db_grasp.hand_name_.get() = grasp->HandName();
00298   db_grasp.table_clearance_.get() = grasp->TableClearance();
00299   db_grasp.compliant_copy_.get() = grasp->CompliantCopy();
00300   db_grasp.compliant_original_id_.get() = grasp->CompliantOriginalId();
00301   db_grasp.scaled_quality_.data() = std::max(0.0, 1.0 - std::max(0.0, grasp->Energy()) / 68.6);
00302 
00303   std::stringstream str;
00304   str << grasp->GetPregraspJoints();
00305   str >> db_grasp.pre_grasp_posture_.get();
00306   if (str.fail()) return false;
00307   str << grasp->GetPregraspPosition();
00308   str >> db_grasp.pre_grasp_pose_.get();
00309   if (str.fail()) return false;
00310   //convert from Graspit units to ROS units
00311   db_grasp.pre_grasp_pose_.get().pose_.position.x /= 1000;
00312   db_grasp.pre_grasp_pose_.get().pose_.position.y /= 1000;
00313   db_grasp.pre_grasp_pose_.get().pose_.position.z /= 1000;
00314 
00315   str << grasp->GetFinalgraspJoints();
00316   str >> db_grasp.final_grasp_posture_.get();
00317   if (str.fail()) return false;
00318   str << grasp->GetFinalgraspPosition();
00319   str >> db_grasp.final_grasp_pose_.get();
00320   if (str.fail()) return false;
00321   //convert from Graspit units to ROS units
00322   db_grasp.final_grasp_pose_.get().pose_.position.x /= 1000;
00323   db_grasp.final_grasp_pose_.get().pose_.position.y /= 1000;
00324   db_grasp.final_grasp_pose_.get().pose_.position.z /= 1000;
00325 
00326   if ( !database_->insertIntoDatabase(&db_grasp) ) return false;
00327   return true;
00328 }
00329 
00330 bool RosDatabaseManager::DeleteGrasp(Grasp* grasp) const
00331 {
00332   household_objects_database::DatabaseGrasp db_grasp;
00333   db_grasp.id_.get() = grasp->GraspId();
00334   if ( !database_->deleteFromDatabase(&db_grasp) ) return false;
00335   return true;
00336 }
00337 
00338 bool RosDatabaseManager::SetGraspClusterRep(Grasp *grasp, bool rep) const
00339 {
00340   household_objects_database::DatabaseGrasp db_grasp;
00341   db_grasp.id_.get() = grasp->GraspId();
00342   db_grasp.cluster_rep_.get() = rep;
00343   if ( !database_->saveToDatabase(&db_grasp.cluster_rep_) ) return false;
00344   return true;
00345 }
00346 bool RosDatabaseManager::SetGraspTableClearance(Grasp *grasp, double clearance) const
00347 {
00348   household_objects_database::DatabaseGrasp db_grasp;
00349   db_grasp.id_.get() = grasp->GraspId();
00350   db_grasp.table_clearance_.get() = clearance;
00351   if ( !database_->saveToDatabase(&db_grasp.table_clearance_) ) return false;
00352   return true;
00353 }
00354 
00355 bool RosDatabaseManager::InsertGraspPair(const Grasp *grasp1, const Grasp *grasp2) const
00356 {
00357   //grasp pair not used anymore
00358   return false;
00359   /*
00360   household_objects_database::DatabaseGraspPair db_pair;
00361   db_pair.grasp1_id_.get() = grasp1->GraspId();
00362   db_pair.grasp2_id_.get() = grasp2->GraspId();
00363   if ( !database_->insertIntoDatabase(&db_pair) ) return false;
00364   return true;
00365   */
00366 }
00367 
00368 bool RosDatabaseManager::LoadModelGeometry(Model* model) const
00369 {
00370   household_objects_database::DatabaseMesh mesh;
00371   if (!database_->getScaledModelMesh(model->ModelId(), mesh)) return false;
00372   //this gets the geometry in ROS units, which are meters
00373   //scale to mm
00374   for (size_t i=0; i<mesh.vertices_.data().size(); i++) {
00375     mesh.vertices_.data().at(i) = 1000 * mesh.vertices_.data().at(i);
00376   }
00377   model->SetGeometry(mesh.vertices_.data(), mesh.triangles_.data());
00378   return true;
00379 }
00380 
00381 } //namespace db_planner