00001
00011
00012 #include "graspdb/Client.h"
00013
00014
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
00027 if (c.connected())
00028 {
00029 this->connect();
00030 }
00031
00032
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
00043 this->checkAPIVersion();
00044 }
00045
00046 Client::~Client()
00047 {
00048
00049 this->disconnect();
00050 }
00051
00052 void Client::checkAPIVersion() const
00053 {
00054
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
00093 this->disconnect();
00094
00095 try
00096 {
00097
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
00106 connection_->prepare("pg_type.exists", "SELECT EXISTS (SELECT 1 FROM pg_type WHERE typname=$1)");
00107
00108
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
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
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
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
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
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
00190 pqxx::work w(*connection_);
00191
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
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
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
00225 w.commit();
00226 }
00227
00228 bool Client::doesTypeExist(const string &type) const
00229 {
00230 pqxx::work w(*connection_);
00231
00232 pqxx::result result = w.prepared("pg_type.exists")(type).exec();
00233 w.commit();
00234
00235 return result[0][0].as<bool>();
00236 }
00237
00238 bool Client::loadGraspDemonstration(uint32_t id, GraspDemonstration &gd) const
00239 {
00240
00241 pqxx::work w(*connection_);
00242 pqxx::result result = w.prepared("grasp_demonstrations.select")(id).exec();
00243 w.commit();
00244
00245
00246 if (result.empty())
00247 {
00248 return false;
00249 } else
00250 {
00251
00252 gd = this->extractGraspDemonstrationFromTuple(result[0]);
00253 return true;
00254 }
00255 }
00256
00257 bool Client::loadGraspDemonstrations(vector<GraspDemonstration> &gds) const
00258 {
00259
00260 pqxx::work w(*connection_);
00261 pqxx::result result = w.prepared("grasp_demonstrations.select_all").exec();
00262 w.commit();
00263
00264
00265 if (result.empty())
00266 {
00267 return false;
00268 } else
00269 {
00270
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
00282 pqxx::work w(*connection_);
00283 pqxx::result result = w.prepared("grasp_demonstrations.select_object_name")(object_name).exec();
00284 w.commit();
00285
00286
00287 if (result.empty())
00288 {
00289 return false;
00290 } else
00291 {
00292
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
00304 pqxx::work w(*connection_);
00305 pqxx::result result = w.prepared("grasps.select")(id).exec();
00306 w.commit();
00307
00308
00309 if (result.empty())
00310 {
00311 return false;
00312 } else
00313 {
00314
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
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
00328 if (result.empty())
00329 {
00330 return false;
00331 } else
00332 {
00333
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
00345 pqxx::work w(*connection_);
00346 pqxx::result result = w.prepared("grasp_models.select")(id).exec();
00347 w.commit();
00348
00349
00350 if (result.empty())
00351 {
00352 return false;
00353 } else
00354 {
00355
00356 gm = this->extractGraspModelFromTuple(result[0]);
00357
00358 vector<Grasp> grasps;
00359 this->loadGraspByGraspModelID(id, grasps);
00360
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
00372 pqxx::work w(*connection_);
00373 pqxx::result result = w.prepared("grasp_models.select_all").exec();
00374 w.commit();
00375
00376
00377 if (result.empty())
00378 {
00379 return false;
00380 } else
00381 {
00382
00383 for (size_t i = 0; i < result.size(); i++)
00384 {
00385 GraspModel gm = this->extractGraspModelFromTuple(result[i]);
00386
00387 vector<Grasp> grasps;
00388 this->loadGraspByGraspModelID(gm.getID(), grasps);
00389
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
00403 pqxx::work w(*connection_);
00404 pqxx::result result = w.prepared("grasp_models.select_object_name")(object_name).exec();
00405 w.commit();
00406
00407
00408 if (result.empty())
00409 {
00410 return false;
00411 } else
00412 {
00413
00414 for (size_t i = 0; i < result.size(); i++)
00415 {
00416 GraspModel gm = this->extractGraspModelFromTuple(result[i]);
00417
00418 vector<Grasp> grasps;
00419 this->loadGraspByGraspModelID(gm.getID(), grasps);
00420
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
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
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
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
00469 #if PQXX_VERSION_MAJOR >= 4
00470
00471
00472
00473 bool Client::addGraspDemonstration(GraspDemonstration &gd) const
00474 {
00475
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
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
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
00503 const string &object_name = gm.getObjectName();
00504 pqxx::binarystring pc = this->toBinaryString(gm.getPointCloud());
00505
00506
00507 pqxx::work w(*connection_);
00508 pqxx::result result = w.prepared("grasp_models.insert")(object_name)(pc).exec();
00509 w.commit();
00510
00511
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
00518 vector<Grasp> addedGrasps;
00519
00520
00521
00522 for (size_t i = 0; i < gm.getNumGrasps(); i++)
00523 {
00524
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
00534 while (gm.getNumGrasps() > 0)
00535 {
00536 gm.removeGrasp(0);
00537 }
00538
00539
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
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
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
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
00595 GraspDemonstration gd;
00596
00597
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
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
00608 Pose pose(tuple["robot_fixed_frame_id"].as<string>(), pos, ori);
00609
00610
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
00618 if (tuple["point_cloud"].size() > 0)
00619 {
00620 pqxx::binarystring blob(tuple["point_cloud"]);
00621 gd.setPointCloud(this->extractPointCloud2FromBinaryString(blob));
00622 }
00623
00624
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
00637 Grasp grasp;
00638
00639
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
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
00650 Pose pose(tuple["robot_fixed_frame_id"].as<string>(), pos, ori);
00651
00652
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
00667 GraspModel gm;
00668
00669
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
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
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
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
00705 vector<double> values;
00706
00707
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
00713 stringstream ss(array);
00714 string str;
00715 double dbl;
00716 while (getline(ss, str, ','))
00717 {
00718
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
00730 struct tm t;
00731 bzero(&t, sizeof(t));
00732
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
00737 t.tm_year -= 1900;
00738 t.tm_mon -= 1;
00739
00740 t.tm_hour += tz;
00741
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
00749 pqxx::work w(*connection_);
00750 pqxx::result result = w.prepared(prepared_name).exec();
00751 w.commit();
00752
00753
00754 if (result.empty())
00755 {
00756 return false;
00757 } else
00758 {
00759
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
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
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
00787 stringstream ss;
00788 ss << "{" << o.getX() << "," << o.getY() << ", " << o.getZ() << "," << o.getW() << "}";
00789 return ss.str();
00790 }
00791
00792
00793 #if PQXX_VERSION_MAJOR >= 4
00794
00795
00796
00797 pqxx::binarystring Client::toBinaryString(const sensor_msgs::PointCloud2 &pc) const
00798 {
00799
00800 uint32_t size = ros::serialization::serializationLength(pc);
00801 uint8_t buffer[size];
00802
00803
00804 ros::serialization::OStream stream(buffer, size);
00805 ros::serialization::serialize(stream, pc);
00806
00807
00808 pqxx::binarystring binary(buffer, size);
00809 return binary;
00810 }
00811
00812 pqxx::binarystring Client::toBinaryString(const sensor_msgs::Image &image) const
00813 {
00814
00815 uint32_t size = ros::serialization::serializationLength(image);
00816 uint8_t buffer[size];
00817
00818
00819 ros::serialization::OStream stream(buffer, size);
00820 ros::serialization::serialize(stream, image);
00821
00822
00823 pqxx::binarystring binary(buffer, size);
00824 return binary;
00825 }
00826
00827 #endif