Client.cpp
Go to the documentation of this file.
00001 
00011 // graspdb
00012 #include "graspdb/Client.h"
00013 
00014 // ROS
00015 #include <ros/ros.h>
00016 
00017 using namespace std;
00018 using namespace rail::pick_and_place::graspdb;
00019 
00020 Client::Client(const Client &c)
00021     : host_(c.getHost()), user_(c.getUser()), password_(c.getPassword()), db_(c.getDatabase())
00022 {
00023   port_ = c.getPort();
00024   connection_ = NULL;
00025 
00026   // check if a connection was made
00027   if (c.connected())
00028   {
00029     this->connect();
00030   }
00031 
00032   // check API versions
00033   this->checkAPIVersion();
00034 }
00035 
00036 Client::Client(const string &host, const uint16_t port, const string &user, const string &password, const string &db) :
00037     host_(host), user_(user), password_(password), db_(db)
00038 {
00039   port_ = port;
00040   connection_ = NULL;
00041 
00042   // check API versions
00043   this->checkAPIVersion();
00044 }
00045 
00046 Client::~Client()
00047 {
00048   // check for an existing connection
00049   this->disconnect();
00050 }
00051 
00052 void Client::checkAPIVersion() const
00053 {
00054   // check API versions
00055 #if PQXX_VERSION_MAJOR < 4
00056   ROS_WARN("libpqxx-%s is not fully supported. Please upgrade to libpqxx-4.0 or greater.", PQXX_VERSION);
00057 #endif
00058 }
00059 
00060 uint16_t Client::getPort() const
00061 {
00062   return port_;
00063 }
00064 
00065 const string &Client::getHost() const
00066 {
00067   return host_;
00068 }
00069 
00070 const string &Client::getUser() const
00071 {
00072   return user_;
00073 }
00074 
00075 const string &Client::getPassword() const
00076 {
00077   return password_;
00078 }
00079 
00080 const string &Client::getDatabase() const
00081 {
00082   return db_;
00083 }
00084 
00085 bool Client::connected() const
00086 {
00087   return connection_ != NULL && connection_->is_open();
00088 }
00089 
00090 bool Client::connect()
00091 {
00092   // check for an existing connection
00093   this->disconnect();
00094 
00095   try
00096   {
00097     // build the connection
00098     stringstream ss;
00099     ss << "dbname=" << db_ << " user=" << user_ << " password=" << password_;
00100     ss << " hostaddr=" << host_ << " port=" << port_;
00101     connection_ = new pqxx::connection(ss.str());
00102 
00103     if (this->connected())
00104     {
00105       // general statements
00106       connection_->prepare("pg_type.exists", "SELECT EXISTS (SELECT 1 FROM pg_type WHERE typname=$1)");
00107 
00108       // grasp_demonstrations statements
00109       connection_->prepare("grasp_demonstrations.delete", "DELETE FROM grasp_demonstrations WHERE id=$1");
00110       connection_->prepare("grasp_demonstrations.insert",
00111                            "INSERT INTO grasp_demonstrations " \
00112                            "(object_name, grasp_pose, eef_frame_id, point_cloud, image) " \
00113                            "VALUES (UPPER($1), $2, $3, $4, $5) RETURNING id, created");
00114       connection_->prepare("grasp_demonstrations.select",
00115                            "SELECT id, object_name, (grasp_pose).robot_fixed_frame_id, (grasp_pose).position, " \
00116           "(grasp_pose).orientation, eef_frame_id, point_cloud, image, created FROM grasp_demonstrations WHERE id=$1");
00117       connection_->prepare("grasp_demonstrations.select_all",
00118                            "SELECT id, object_name, (grasp_pose).robot_fixed_frame_id, (grasp_pose).position, " \
00119           "(grasp_pose).orientation, eef_frame_id, point_cloud, image, created FROM grasp_demonstrations");
00120       connection_->prepare("grasp_demonstrations.select_object_name",
00121                            "SELECT id, object_name, (grasp_pose).robot_fixed_frame_id, (grasp_pose).position, " \
00122           "(grasp_pose).orientation, eef_frame_id, point_cloud, image, created " \
00123           "FROM grasp_demonstrations WHERE UPPER(object_name)=UPPER($1)");
00124       connection_->prepare("grasp_demonstrations.unique", "SELECT DISTINCT object_name FROM grasp_demonstrations");
00125 
00126       // grasp_models statements
00127       connection_->prepare("grasp_models.delete", "DELETE FROM grasp_models WHERE id=$1");
00128       connection_->prepare("grasp_models.insert", "INSERT INTO grasp_models (object_name, point_cloud) " \
00129                            "VALUES (UPPER($1), $2) RETURNING id, created");
00130       connection_->prepare("grasp_models.select",
00131                            "SELECT id, object_name, point_cloud, created FROM grasp_models WHERE id=$1");
00132       connection_->prepare("grasp_models.select_all", "SELECT id, object_name, point_cloud, created FROM grasp_models");
00133       connection_->prepare("grasp_models.select_object_name", "SELECT id, object_name, point_cloud, created " \
00134                            "FROM grasp_models WHERE UPPER(object_name)=UPPER($1)");
00135       connection_->prepare("grasp_models.unique", "SELECT DISTINCT object_name FROM grasp_models");
00136 
00137       // grasps statements
00138       connection_->prepare("grasps.delete", "DELETE FROM grasps WHERE id=$1");
00139       connection_->prepare("grasps.insert",
00140                            "INSERT INTO grasps (grasp_model_id, grasp_pose, eef_frame_id, successes, attempts) " \
00141           "VALUES ($1, $2, $3, $4, $5) RETURNING id, created");
00142       connection_->prepare("grasps.select",
00143                            "SELECT id, grasp_model_id, (grasp_pose).robot_fixed_frame_id, (grasp_pose).position, " \
00144           "(grasp_pose).orientation, eef_frame_id, successes, attempts, created FROM grasps WHERE id=$1");
00145       connection_->prepare("grasps.select_grasp_model_id",
00146                            "SELECT id, grasp_model_id, (grasp_pose).robot_fixed_frame_id, (grasp_pose).position, " \
00147           "(grasp_pose).orientation, eef_frame_id, successes, attempts, created FROM grasps  WHERE grasp_model_id=$1");
00148 
00149       // create the tables in the DB if they do not exist
00150       this->createTables();
00151     }
00152   } catch (const exception &e)
00153   {
00154     ROS_ERROR("%s", e.what());
00155   }
00156 
00157   return this->connected();
00158 }
00159 
00160 void Client::disconnect()
00161 {
00162   // check for an existing connection
00163   if (connection_ != NULL)
00164   {
00165     if (this->connected())
00166     {
00167       connection_->disconnect();
00168     }
00169     delete connection_;
00170     connection_ = NULL;
00171   }
00172 }
00173 
00174 void Client::createTables() const
00175 {
00176   // check for and create the pose type
00177   if (!this->doesTypeExist("pose"))
00178   {
00179     pqxx::work w(*connection_);
00180     string sql = "CREATE TYPE pose AS (" \
00181                    "robot_fixed_frame_id VARCHAR," \
00182                    "position NUMERIC[3]," \
00183                    "orientation NUMERIC[4]" \
00184                  ");";
00185     w.exec(sql);
00186     w.commit();
00187   }
00188 
00189   // shared worker
00190   pqxx::work w(*connection_);
00191   // create the grasp_demonstrations table if it doesn't exist
00192   string grasp_demonstrations_sql = "CREATE TABLE IF NOT EXISTS grasp_demonstrations (" \
00193                                    "id SERIAL PRIMARY KEY," \
00194                                    "object_name VARCHAR NOT NULL," \
00195                                    "grasp_pose pose NOT NULL," \
00196                                    "eef_frame_id VARCHAR NOT NULL," \
00197                                    "point_cloud BYTEA NOT NULL," \
00198                                    "image BYTEA NOT NULL," \
00199                                    "created TIMESTAMP WITH TIME ZONE NOT NULL DEFAULT NOW()" \
00200                                  ");";
00201   w.exec(grasp_demonstrations_sql);
00202 
00203   // create the grasp models table if it doesn't exist
00204   string grasp_models_sql = "CREATE TABLE IF NOT EXISTS grasp_models (" \
00205                               "id SERIAL PRIMARY KEY," \
00206                               "object_name VARCHAR NOT NULL," \
00207                               "point_cloud BYTEA NOT NULL," \
00208                               "created TIMESTAMP WITH TIME ZONE NOT NULL DEFAULT NOW()" \
00209                             ");";
00210   w.exec(grasp_models_sql);
00211 
00212   // create the grasps table if it doesn't exist
00213   string grasps_sql = "CREATE TABLE IF NOT EXISTS grasps (" \
00214                         "id SERIAL PRIMARY KEY," \
00215                         "grasp_model_id INTEGER NOT NULL REFERENCES grasp_models(id) ON DELETE CASCADE," \
00216                         "grasp_pose pose NOT NULL," \
00217                         "eef_frame_id VARCHAR NOT NULL," \
00218                         "successes INTEGER NOT NULL," \
00219                         "attempts INTEGER NOT NULL," \
00220                         "created TIMESTAMP WITH TIME ZONE NOT NULL DEFAULT NOW()" \
00221                       ");";
00222   w.exec(grasps_sql);
00223 
00224   // commit the changes
00225   w.commit();
00226 }
00227 
00228 bool Client::doesTypeExist(const string &type) const
00229 {
00230   pqxx::work w(*connection_);
00231   // create and execute the query
00232   pqxx::result result = w.prepared("pg_type.exists")(type).exec();
00233   w.commit();
00234   // return the result
00235   return result[0][0].as<bool>();
00236 }
00237 
00238 bool Client::loadGraspDemonstration(uint32_t id, GraspDemonstration &gd) const
00239 {
00240   // create and execute the query
00241   pqxx::work w(*connection_);
00242   pqxx::result result = w.prepared("grasp_demonstrations.select")(id).exec();
00243   w.commit();
00244 
00245   // check the result
00246   if (result.empty())
00247   {
00248     return false;
00249   } else
00250   {
00251     // extract the information
00252     gd = this->extractGraspDemonstrationFromTuple(result[0]);
00253     return true;
00254   }
00255 }
00256 
00257 bool Client::loadGraspDemonstrations(vector<GraspDemonstration> &gds) const
00258 {
00259   // create and execute the query
00260   pqxx::work w(*connection_);
00261   pqxx::result result = w.prepared("grasp_demonstrations.select_all").exec();
00262   w.commit();
00263 
00264   // check the result
00265   if (result.empty())
00266   {
00267     return false;
00268   } else
00269   {
00270     // extract each result
00271     for (size_t i = 0; i < result.size(); i++)
00272     {
00273       gds.push_back(this->extractGraspDemonstrationFromTuple(result[i]));
00274     }
00275     return true;
00276   }
00277 }
00278 
00279 bool Client::loadGraspDemonstrationsByObjectName(const string &object_name, vector<GraspDemonstration> &gds) const
00280 {
00281   // create and execute the query
00282   pqxx::work w(*connection_);
00283   pqxx::result result = w.prepared("grasp_demonstrations.select_object_name")(object_name).exec();
00284   w.commit();
00285 
00286   // check the result
00287   if (result.empty())
00288   {
00289     return false;
00290   } else
00291   {
00292     // extract each result
00293     for (size_t i = 0; i < result.size(); i++)
00294     {
00295       gds.push_back(this->extractGraspDemonstrationFromTuple(result[i]));
00296     }
00297     return true;
00298   }
00299 }
00300 
00301 bool Client::loadGrasp(uint32_t id, Grasp &grasp) const
00302 {
00303   // create and execute the query
00304   pqxx::work w(*connection_);
00305   pqxx::result result = w.prepared("grasps.select")(id).exec();
00306   w.commit();
00307 
00308   // check the result
00309   if (result.empty())
00310   {
00311     return false;
00312   } else
00313   {
00314     // extract the information
00315     grasp = this->extractGraspFromTuple(result[0]);
00316     return true;
00317   }
00318 }
00319 
00320 bool Client::loadGraspByGraspModelID(const uint32_t grasp_model_id, vector<Grasp> &grasps) const
00321 {
00322   // create and execute the query
00323   pqxx::work w(*connection_);
00324   pqxx::result result = w.prepared("grasps.select_grasp_model_id")(grasp_model_id).exec();
00325   w.commit();
00326 
00327   // check the result
00328   if (result.empty())
00329   {
00330     return false;
00331   } else
00332   {
00333     // extract each result
00334     for (size_t i = 0; i < result.size(); i++)
00335     {
00336       grasps.push_back(this->extractGraspFromTuple(result[i]));
00337     }
00338     return true;
00339   }
00340 }
00341 
00342 bool Client::loadGraspModel(uint32_t id, GraspModel &gm) const
00343 {
00344   // create and execute the query
00345   pqxx::work w(*connection_);
00346   pqxx::result result = w.prepared("grasp_models.select")(id).exec();
00347   w.commit();
00348 
00349   // check the result
00350   if (result.empty())
00351   {
00352     return false;
00353   } else
00354   {
00355     // extract the information
00356     gm = this->extractGraspModelFromTuple(result[0]);
00357     // now load the grasps
00358     vector<Grasp> grasps;
00359     this->loadGraspByGraspModelID(id, grasps);
00360     // add each grasp
00361     for (size_t i = 0; i < grasps.size(); i++)
00362     {
00363       gm.addGrasp(grasps[i]);
00364     }
00365     return true;
00366   }
00367 }
00368 
00369 bool Client::loadGraspModels(vector<GraspModel> &gms) const
00370 {
00371   // create and execute the query
00372   pqxx::work w(*connection_);
00373   pqxx::result result = w.prepared("grasp_models.select_all").exec();
00374   w.commit();
00375 
00376   // check the result
00377   if (result.empty())
00378   {
00379     return false;
00380   } else
00381   {
00382     // extract each result
00383     for (size_t i = 0; i < result.size(); i++)
00384     {
00385       GraspModel gm = this->extractGraspModelFromTuple(result[i]);
00386       // now load the grasps
00387       vector<Grasp> grasps;
00388       this->loadGraspByGraspModelID(gm.getID(), grasps);
00389       // add each grasp
00390       for (size_t i = 0; i < grasps.size(); i++)
00391       {
00392         gm.addGrasp(grasps[i]);
00393       }
00394       gms.push_back(gm);
00395     }
00396     return true;
00397   }
00398 }
00399 
00400 bool Client::loadGraspModelsByObjectName(const string &object_name, vector<GraspModel> &gms) const
00401 {
00402   // create and execute the query
00403   pqxx::work w(*connection_);
00404   pqxx::result result = w.prepared("grasp_models.select_object_name")(object_name).exec();
00405   w.commit();
00406 
00407   // check the result
00408   if (result.empty())
00409   {
00410     return false;
00411   } else
00412   {
00413     // extract each result
00414     for (size_t i = 0; i < result.size(); i++)
00415     {
00416       GraspModel gm = this->extractGraspModelFromTuple(result[i]);
00417       // now load the grasps
00418       vector<Grasp> grasps;
00419       this->loadGraspByGraspModelID(gm.getID(), grasps);
00420       // add each grasp
00421       for (size_t i = 0; i < grasps.size(); i++)
00422       {
00423         gm.addGrasp(grasps[i]);
00424       }
00425       gms.push_back(gm);
00426     }
00427     return true;
00428   }
00429 }
00430 
00431 bool Client::getUniqueGraspDemonstrationObjectNames(vector<string> &names) const
00432 {
00433   return this->getStringArrayFromPrepared("grasp_demonstrations.unique", "object_name", names);
00434 }
00435 
00436 bool Client::getUniqueGraspModelObjectNames(vector<string> &names) const
00437 {
00438   return this->getStringArrayFromPrepared("grasp_models.unique", "object_name", names);
00439 }
00440 
00441 bool Client::addGrasp(Grasp &grasp) const
00442 {
00443   // build the SQL bits we need
00444   uint32_t grasp_model_id = grasp.getGraspModelID();
00445   const string &grasp_pose = this->toSQL(grasp.getGraspPose());
00446   const string &eef_frame_id = grasp.getEefFrameID();
00447   uint32_t succeses = grasp.getSuccesses();
00448   uint32_t attempts = grasp.getAttempts();
00449 
00450   // create and execute the query
00451   pqxx::work w(*connection_);
00452   pqxx::result result = w.prepared("grasps.insert")(grasp_model_id)(grasp_pose)(eef_frame_id)(succeses)(attempts)
00453       .exec();
00454   w.commit();
00455 
00456   // check the result
00457   if (!result.empty())
00458   {
00459     grasp.setID(result[0]["id"].as<uint32_t>());
00460     grasp.setCreated(this->extractTimeFromString(result[0]["created"].as<string>()));
00461     return true;
00462   } else
00463   {
00464     return false;
00465   }
00466 }
00467 
00468 // check API versions
00469 #if PQXX_VERSION_MAJOR >= 4
00470 
00471 /* Only pqxx 4.0.0 or greater support insert with binary strings */
00472 
00473 bool Client::addGraspDemonstration(GraspDemonstration &gd) const
00474 {
00475   // build the SQL bits we need
00476   const string &object_name = gd.getObjectName();
00477   string grasp_pose = this->toSQL(gd.getGraspPose());
00478   const string &eef_frame_id = gd.getEefFrameID();
00479   pqxx::binarystring pc = this->toBinaryString(gd.getPointCloud());
00480   pqxx::binarystring image = this->toBinaryString(gd.getImage());
00481 
00482   // create and execute the query
00483   pqxx::work w(*connection_);
00484   pqxx::result result = w.prepared("grasp_demonstrations.insert")(object_name)(grasp_pose)(eef_frame_id)(pc)(image)
00485       .exec();
00486   w.commit();
00487 
00488   // check the result
00489   if (!result.empty())
00490   {
00491     gd.setID(result[0]["id"].as<uint32_t>());
00492     gd.setCreated(this->extractTimeFromString(result[0]["created"].as<string>()));
00493     return true;
00494   } else
00495   {
00496     return false;
00497   }
00498 }
00499 
00500 bool Client::addGraspModel(GraspModel &gm) const
00501 {
00502   // build the SQL bits we need
00503   const string &object_name = gm.getObjectName();
00504   pqxx::binarystring pc = this->toBinaryString(gm.getPointCloud());
00505 
00506   // create and execute the query
00507   pqxx::work w(*connection_);
00508   pqxx::result result = w.prepared("grasp_models.insert")(object_name)(pc).exec();
00509   w.commit();
00510 
00511   // check the result
00512   if (!result.empty())
00513   {
00514     gm.setID(result[0]["id"].as<uint32_t>());
00515     gm.setCreated(this->extractTimeFromString(result[0]["created"].as<string>()));
00516 
00517     // used to add back the grasps with the correct ID and created fields
00518     vector<Grasp> addedGrasps;
00519 
00520     // now add each grasp to the database
00521 
00522     for (size_t i = 0; i < gm.getNumGrasps(); i++)
00523     {
00524       // set the model ID and attempt to add it
00525       Grasp grasp = gm.getGrasp(i);
00526       grasp.setGraspModelID(gm.getID());
00527       if (this->addGrasp(grasp))
00528       {
00529         addedGrasps.push_back(grasp);
00530       }
00531     }
00532 
00533     // clear out the old grasp values
00534     while (gm.getNumGrasps() > 0)
00535     {
00536       gm.removeGrasp(0);
00537     }
00538 
00539     // add each new grasp
00540     for (size_t i = 0; i < addedGrasps.size(); i++)
00541     {
00542       gm.addGrasp(addedGrasps[i]);
00543     }
00544 
00545     return true;
00546   } else
00547   {
00548     return false;
00549   }
00550 }
00551 
00552 #else
00553 
00554 bool Client::addGraspModel(GraspModel &gm) const
00555 {
00556   ROS_WARN("libpqxx-%s does not support binary string insertion. Add grasp model ignored.", PQXX_VERSION);
00557   return false;
00558 }
00559 
00560 bool Client::addGraspDemonstration(GraspDemonstration &gd) const
00561 {
00562   ROS_WARN("libpqxx-%s does not support binary string insertion. Add grasp demonstration ignored.", PQXX_VERSION);
00563   return false;
00564 }
00565 
00566 #endif
00567 
00568 void Client::deleteGrasp(uint32_t id) const
00569 {
00570   // create and execute the query
00571   pqxx::work w(*connection_);
00572   pqxx::result result = w.prepared("grasps.delete")(id).exec();
00573   w.commit();
00574 }
00575 
00576 void Client::deleteGraspDemonstration(uint32_t id) const
00577 {
00578   // create and execute the query
00579   pqxx::work w(*connection_);
00580   pqxx::result result = w.prepared("grasp_demonstrations.delete")(id).exec();
00581   w.commit();
00582 }
00583 
00584 void Client::deleteGraspModel(uint32_t id) const
00585 {
00586   // create and execute the query
00587   pqxx::work w(*connection_);
00588   pqxx::result result = w.prepared("grasp_models.delete")(id).exec();
00589   w.commit();
00590 }
00591 
00592 GraspDemonstration Client::extractGraspDemonstrationFromTuple(const pqxx::result::tuple &tuple) const
00593 {
00594   // to return
00595   GraspDemonstration gd;
00596 
00597   // create the Position element
00598   string position_string = tuple["position"].as<string>();
00599   vector<double> position_values = this->extractArrayFromString(position_string);
00600   Position pos(position_values[0], position_values[1], position_values[2]);
00601 
00602   // create the Orientation element
00603   string orientation_string = tuple["orientation"].as<string>();
00604   vector<double> orientation_values = this->extractArrayFromString(orientation_string);
00605   Orientation ori(orientation_values[0], orientation_values[1], orientation_values[2], orientation_values[3]);
00606 
00607   // create the Pose element
00608   Pose pose(tuple["robot_fixed_frame_id"].as<string>(), pos, ori);
00609 
00610   // set our fields
00611   gd.setID(tuple["id"].as<uint32_t>());
00612   gd.setObjectName(tuple["object_name"].as<string>());
00613   gd.setGraspPose(pose);
00614   gd.setEefFrameID(tuple["eef_frame_id"].as<string>());
00615   gd.setCreated(this->extractTimeFromString(tuple["created"].as<string>()));
00616 
00617   // extract the point cloud if there is one
00618   if (tuple["point_cloud"].size() > 0)
00619   {
00620     pqxx::binarystring blob(tuple["point_cloud"]);
00621     gd.setPointCloud(this->extractPointCloud2FromBinaryString(blob));
00622   }
00623 
00624   // extract the image if there is one
00625   if (tuple["image"].size() > 0)
00626   {
00627     pqxx::binarystring blob(tuple["image"]);
00628     gd.setImage(this->extractImageFromBinaryString(blob));
00629   }
00630 
00631   return gd;
00632 }
00633 
00634 Grasp Client::extractGraspFromTuple(const pqxx::result::tuple &tuple) const
00635 {
00636   // to return
00637   Grasp grasp;
00638 
00639   // create the Position element
00640   string position_string = tuple["position"].as<string>();
00641   vector<double> position_values = this->extractArrayFromString(position_string);
00642   Position pos(position_values[0], position_values[1], position_values[2]);
00643 
00644   // create the Orientation element
00645   string orientation_string = tuple["orientation"].as<string>();
00646   vector<double> orientation_values = this->extractArrayFromString(orientation_string);
00647   Orientation ori(orientation_values[0], orientation_values[1], orientation_values[2], orientation_values[3]);
00648 
00649   // create the Pose element
00650   Pose pose(tuple["robot_fixed_frame_id"].as<string>(), pos, ori);
00651 
00652   // set our fields
00653   grasp.setID(tuple["id"].as<uint32_t>());
00654   grasp.setGraspModelID(tuple["grasp_model_id"].as<uint32_t>());
00655   grasp.setGraspPose(pose);
00656   grasp.setEefFrameID(tuple["eef_frame_id"].as<string>());
00657   grasp.setSuccesses(tuple["successes"].as<uint32_t>());
00658   grasp.setAttempts(tuple["attempts"].as<uint32_t>());
00659   grasp.setCreated(this->extractTimeFromString(tuple["created"].as<string>()));
00660 
00661   return grasp;
00662 }
00663 
00664 GraspModel Client::extractGraspModelFromTuple(const pqxx::result::tuple &tuple) const
00665 {
00666   // to return
00667   GraspModel gm;
00668 
00669   // set our fields
00670   gm.setID(tuple["id"].as<uint32_t>());
00671   gm.setObjectName(tuple["object_name"].as<string>());
00672   gm.setCreated(this->extractTimeFromString(tuple["created"].as<string>()));
00673 
00674   // extract the point cloud if there is one
00675   if (tuple["point_cloud"].size() > 0)
00676   {
00677     pqxx::binarystring blob(tuple["point_cloud"]);
00678     gm.setPointCloud(this->extractPointCloud2FromBinaryString(blob));
00679   }
00680 
00681   return gm;
00682 }
00683 
00684 sensor_msgs::PointCloud2 Client::extractPointCloud2FromBinaryString(const pqxx::binarystring &bs) const
00685 {
00686   sensor_msgs::PointCloud2 pc;
00687   // deserialize from memory
00688   ros::serialization::IStream stream((uint8_t *) bs.data(), bs.size());
00689   ros::serialization::Serializer<sensor_msgs::PointCloud2>::read(stream, pc);
00690   return pc;
00691 }
00692 
00693 sensor_msgs::Image Client::extractImageFromBinaryString(const pqxx::binarystring &bs) const
00694 {
00695   sensor_msgs::Image image;
00696   // deserialize from memory
00697   ros::serialization::IStream stream((uint8_t *) bs.data(), bs.size());
00698   ros::serialization::Serializer<sensor_msgs::Image>::read(stream, image);
00699   return image;
00700 }
00701 
00702 vector<double> Client::extractArrayFromString(string &array) const
00703 {
00704   // to return
00705   vector<double> values;
00706 
00707   // remove the brackets and spaces
00708   array.erase(remove(array.begin(), array.end(), '{'), array.end());
00709   array.erase(remove(array.begin(), array.end(), '}'), array.end());
00710   array.erase(remove(array.begin(), array.end(), ' '), array.end());
00711 
00712   // split on the ','
00713   stringstream ss(array);
00714   string str;
00715   double dbl;
00716   while (getline(ss, str, ','))
00717   {
00718     // store as the double value
00719     istringstream i(str);
00720     i >> dbl;
00721     values.push_back(dbl);
00722   }
00723 
00724   return values;
00725 }
00726 
00727 time_t Client::extractTimeFromString(const string &str) const
00728 {
00729   // set values we don't need to be 0
00730   struct tm t;
00731   bzero(&t, sizeof(t));
00732   // extract values in a datetime object and the timezone offset into an int (ignore nsec)
00733   int nsec, tz;
00734   sscanf(str.c_str(), "%d-%d-%d %d:%d:%d.%d%d", &t.tm_year, &t.tm_mon, &t.tm_mday, &t.tm_hour, &t.tm_min, &t.tm_sec,
00735          &nsec, &tz);
00736   // correct the information for C time
00737   t.tm_year -= 1900;
00738   t.tm_mon -= 1;
00739   // fix the timezone offset
00740   t.tm_hour += tz;
00741   // convert to a time_t object
00742   return mktime(&t);
00743 }
00744 
00745 bool Client::getStringArrayFromPrepared(const string &prepared_name, const string &column_name,
00746     vector<string> &strings) const
00747 {
00748   // create and execute the query
00749   pqxx::work w(*connection_);
00750   pqxx::result result = w.prepared(prepared_name).exec();
00751   w.commit();
00752 
00753   // check the result
00754   if (result.empty())
00755   {
00756     return false;
00757   } else
00758   {
00759     // extract each result
00760     for (size_t i = 0; i < result.size(); i++)
00761     {
00762       strings.push_back(result[i][column_name].as<string>());
00763     }
00764     return true;
00765   }
00766 }
00767 
00768 string Client::toSQL(const Pose &p) const
00769 {
00770   // build the SQL
00771   string sql = "(\"" + p.getRobotFixedFrameID() + "\",\"" + this->toSQL(p.getPosition()) + "\",\""
00772                + this->toSQL(p.getOrientation()) + "\")";
00773   return sql;
00774 }
00775 
00776 string Client::toSQL(const Position &p) const
00777 {
00778   // build the SQL
00779   stringstream ss;
00780   ss << "{" << p.getX() << "," << p.getY() << "," << p.getZ() << "}";
00781   return ss.str();
00782 }
00783 
00784 string Client::toSQL(const Orientation &o) const
00785 {
00786   // build the SQL
00787   stringstream ss;
00788   ss << "{" << o.getX() << "," << o.getY() << ", " << o.getZ() << "," << o.getW() << "}";
00789   return ss.str();
00790 }
00791 
00792 // check API versions
00793 #if PQXX_VERSION_MAJOR >= 4
00794 
00795 /* Only pqxx 4.0.0 or greater support insert with binary strings */
00796 
00797 pqxx::binarystring Client::toBinaryString(const sensor_msgs::PointCloud2 &pc) const
00798 {
00799   // determine the size for the buffer
00800   uint32_t size = ros::serialization::serializationLength(pc);
00801   uint8_t buffer[size];
00802 
00803   // serilize the message
00804   ros::serialization::OStream stream(buffer, size);
00805   ros::serialization::serialize(stream, pc);
00806 
00807   // construct a binary string
00808   pqxx::binarystring binary(buffer, size);
00809   return binary;
00810 }
00811 
00812 pqxx::binarystring Client::toBinaryString(const sensor_msgs::Image &image) const
00813 {
00814   // determine the size for the buffer
00815   uint32_t size = ros::serialization::serializationLength(image);
00816   uint8_t buffer[size];
00817 
00818   // serilize the message
00819   ros::serialization::OStream stream(buffer, size);
00820   ros::serialization::serialize(stream, image);
00821 
00822   // construct a binary string
00823   pqxx::binarystring binary(buffer, size);
00824   return binary;
00825 }
00826 
00827 #endif


graspdb
Author(s): Russell Toris
autogenerated on Sun Mar 6 2016 11:38:59