graspit: /opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/DBase/DBPlanner/ros_database

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)
00127 {
00128   std::vector< boost::shared_ptr<household_objects_database::DatabaseTask> > db_task;
00129   if (!database_->acquireNextTask(db_task)) 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 {
00204   household_objects_database::DatabaseOptimizationResult optResult;
00205   optResult.dbase_task_id_.data() = rec.taskId;
00206   optResult.parameters_.data() = parameters;
00207   optResult.hand_name_.data() = rec.hand_name;
00208   optResult.results_.data() = results;
00209   if (!database_->insertIntoDatabase(&optResult))
00210   {
00211     std::cout << "Failed to save optimization results for task id " << rec.taskId << "\n";
00212     return false;
00213   }
00214   return true;
00215 }
00216 
00217 bool RosDatabaseManager::GraspTypeList(vector<string>* type_list) const
00218 {
00219   type_list->push_back("ALL");
00220   return true;
00221 }
00222 
00223 bool RosDatabaseManager::GetGrasps(const Model& model, const string& hand_name, 
00224                                    vector<Grasp*>* grasp_list) const
00225 {
00226   std::vector< boost::shared_ptr<household_objects_database::DatabaseGrasp> > db_grasp_list;
00227   if ( !database_->getGrasps(model.ModelId(), hand_name, db_grasp_list) ) return false;
00228   for (size_t i=0; i<db_grasp_list.size(); i++)
00229   {
00230     Grasp *grasp = grasp_allocator_->Get();
00231     const household_objects_database::DatabaseGrasp &db_grasp = *(db_grasp_list[i]);
00232 
00233     grasp->SetSourceModel(model);
00234     grasp->SetHandName(hand_name);
00235 
00236     grasp->SetGraspId(db_grasp.id_.get());
00237     grasp->SetEnergy(db_grasp.quality_.get());
00238     grasp->SetEpsilonQuality(0.0);
00239     grasp->SetVolumeQuality(0.0);
00240     grasp->SetClearance(db_grasp.pre_grasp_clearance_.get());
00241     grasp->SetClusterRep(db_grasp.cluster_rep_.get());
00242     grasp->SetHandName(db_grasp.hand_name_.get());
00243     grasp->SetTableClearance(db_grasp.table_clearance_.get());
00244     grasp->SetCompliantCopy(db_grasp.compliant_copy_.get());
00245     grasp->SetCompliantOriginalId(db_grasp.compliant_original_id_.get());
00246     
00247     std::stringstream str;
00248     std::vector<double> pregrasp_joints;
00249     str << db_grasp.pre_grasp_posture_.get(); 
00250     str >> pregrasp_joints;
00251     if (str.fail()) return false;
00252     std::vector<double> pregrasp_position;
00253     str << db_grasp.pre_grasp_pose_.get();
00254     str >> pregrasp_position;
00255     if (str.fail()) return false;
00256     std::vector<double> grasp_joints;
00257     str << db_grasp.final_grasp_posture_.get(); 
00258     str >> grasp_joints; 
00259     if (str.fail()) return false;
00260     std::vector<double> grasp_position;
00261     str << db_grasp.final_grasp_pose_.get();
00262     str >> grasp_position;
00263     if (str.fail()) return false;
00264     
00265     //convert from ROS units to Graspit units
00266     pregrasp_position[0] *= 1000;
00267     pregrasp_position[1] *= 1000;
00268     pregrasp_position[2] *= 1000;
00269     grasp_position[0] *= 1000;
00270     grasp_position[1] *= 1000;
00271     grasp_position[2] *= 1000;
00272 
00273     grasp->SetGraspParameters(pregrasp_joints, 
00274                              pregrasp_position, 
00275                              grasp_joints, 
00276                              grasp_position);
00277     grasp->SetPregraspJoints(pregrasp_joints);
00278     grasp->SetPregraspPosition(pregrasp_position);
00279     grasp->SetFinalgraspJoints(grasp_joints);
00280     grasp->SetFinalgraspPosition(grasp_position);
00281     
00282     grasp_list->push_back(grasp);
00283   }
00284   return true;
00285 }
00286 
00287 bool RosDatabaseManager::SaveGrasp(const Grasp* grasp) const
00288 {
00289   household_objects_database::DatabaseGrasp db_grasp;
00290   db_grasp.scaled_model_id_.get() = grasp->SourceModel().ModelId();
00291   db_grasp.quality_.get() = grasp->Energy();
00292   db_grasp.pre_grasp_clearance_.get() = grasp->Clearance();
00293   db_grasp.cluster_rep_.get() = grasp->ClusterRep();
00294   db_grasp.hand_name_.get() = grasp->HandName();
00295   db_grasp.table_clearance_.get() = grasp->TableClearance();
00296   db_grasp.compliant_copy_.get() = grasp->CompliantCopy();
00297   db_grasp.compliant_original_id_.get() = grasp->CompliantOriginalId();
00298   db_grasp.scaled_quality_.data() = std::max(0.0, 1.0 - std::max(0.0, grasp->Energy()) / 68.6);
00299 
00300   std::stringstream str;
00301   str << grasp->GetPregraspJoints();
00302   str >> db_grasp.pre_grasp_posture_.get();
00303   if (str.fail()) return false;
00304   str << grasp->GetPregraspPosition();
00305   str >> db_grasp.pre_grasp_pose_.get();
00306   if (str.fail()) return false;
00307   //convert from Graspit units to ROS units
00308   db_grasp.pre_grasp_pose_.get().pose_.position.x /= 1000;
00309   db_grasp.pre_grasp_pose_.get().pose_.position.y /= 1000;
00310   db_grasp.pre_grasp_pose_.get().pose_.position.z /= 1000;
00311 
00312   str << grasp->GetFinalgraspJoints();
00313   str >> db_grasp.final_grasp_posture_.get();
00314   if (str.fail()) return false;
00315   str << grasp->GetFinalgraspPosition();
00316   str >> db_grasp.final_grasp_pose_.get();
00317   if (str.fail()) return false;
00318   //convert from Graspit units to ROS units
00319   db_grasp.final_grasp_pose_.get().pose_.position.x /= 1000;
00320   db_grasp.final_grasp_pose_.get().pose_.position.y /= 1000;
00321   db_grasp.final_grasp_pose_.get().pose_.position.z /= 1000;
00322 
00323   if ( !database_->insertIntoDatabase(&db_grasp) ) return false;
00324   return true;
00325 }
00326 
00327 bool RosDatabaseManager::DeleteGrasp(Grasp* grasp) const
00328 {
00329   household_objects_database::DatabaseGrasp db_grasp;
00330   db_grasp.id_.get() = grasp->GraspId();
00331   if ( !database_->deleteFromDatabase(&db_grasp) ) return false;
00332   return true;
00333 }
00334 
00335 bool RosDatabaseManager::SetGraspClusterRep(Grasp *grasp, bool rep) const
00336 {
00337   household_objects_database::DatabaseGrasp db_grasp;
00338   db_grasp.id_.get() = grasp->GraspId();
00339   db_grasp.cluster_rep_.get() = rep;
00340   if ( !database_->saveToDatabase(&db_grasp.cluster_rep_) ) return false;
00341   return true;
00342 }
00343 bool RosDatabaseManager::SetGraspTableClearance(Grasp *grasp, double clearance) const
00344 {
00345   household_objects_database::DatabaseGrasp db_grasp;
00346   db_grasp.id_.get() = grasp->GraspId();
00347   db_grasp.table_clearance_.get() = clearance;
00348   if ( !database_->saveToDatabase(&db_grasp.table_clearance_) ) return false;
00349   return true;
00350 }
00351 
00352 bool RosDatabaseManager::InsertGraspPair(const Grasp *grasp1, const Grasp *grasp2) const
00353 {
00354   //grasp pair not used anymore
00355   return false;
00356   /*
00357   household_objects_database::DatabaseGraspPair db_pair;
00358   db_pair.grasp1_id_.get() = grasp1->GraspId();
00359   db_pair.grasp2_id_.get() = grasp2->GraspId();
00360   if ( !database_->insertIntoDatabase(&db_pair) ) return false;
00361   return true;
00362   */
00363 }
00364 
00365 bool RosDatabaseManager::LoadModelGeometry(Model* model) const
00366 {
00367   household_objects_database::DatabaseMesh mesh;
00368   if (!database_->getScaledModelMesh(model->ModelId(), mesh)) return false;
00369   //this gets the geometry in ROS units, which are meters
00370   //scale to mm
00371   for (size_t i=0; i<mesh.vertices_.data().size(); i++) {
00372     mesh.vertices_.data().at(i) = 1000 * mesh.vertices_.data().at(i);
00373   }
00374   model->SetGeometry(mesh.vertices_.data(), mesh.triangles_.data());
00375   return true;
00376 }
00377 
00378 } //namespace db_planner
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/DBase/DBPlanner/ros_database_manager.cpp
