SpatialWorldClient.cpp
Go to the documentation of this file.
00001 
00011 // worldlib
00012 #include "worldlib/remote/SpatialWorldClient.h"
00013 
00014 // ROS
00015 #include <ros/ros.h>
00016 
00017 // Boost
00018 #include <boost/algorithm/string.hpp>
00019 
00020 using namespace std;
00021 using namespace rail::spatial_temporal_learning::worldlib::geometry;
00022 using namespace rail::spatial_temporal_learning::worldlib::model;
00023 using namespace rail::spatial_temporal_learning::worldlib::remote;
00024 using namespace rail::spatial_temporal_learning::worldlib::world;
00025 
00026 SpatialWorldClient::SpatialWorldClient(const SpatialWorldClient &client) : SqlClient(client)
00027 {
00028 }
00029 
00030 SpatialWorldClient::SpatialWorldClient(const string &host, const uint16_t port, const string &user,
00031     const string &password, const string &database) : SqlClient(host, port, user, password, database)
00032 {
00033 }
00034 
00035 bool SpatialWorldClient::connect()
00036 {
00037   bool success = SqlClient::connect();
00038   // attempt to create the tables
00039   this->createTable();
00040   return success;
00041 }
00042 
00043 void SpatialWorldClient::createTable()
00044 {
00045   if (this->connected())
00046   {
00047     // SQL the create the spatial world (sw) table
00048     string sql = "CREATE TABLE IF NOT EXISTS `observations` (" \
00049                    "`id` int(10) unsigned NOT NULL AUTO_INCREMENT, " \
00050                    "`item_name` varchar(255) NOT NULL, " \
00051                    "`surface_name` varchar(255) NOT NULL, " \
00052                    "`surface_frame_id` varchar(255) NOT NULL, " \
00053                    "`x` double NOT NULL, " \
00054                    "`y` double NOT NULL, " \
00055                    "`z` double NOT NULL, " \
00056                    "`theta` double NOT NULL, " \
00057                    "`time` timestamp NOT NULL DEFAULT CURRENT_TIMESTAMP, " \
00058                    "`removed_estimate` timestamp NOT NULL DEFAULT '0000-00-00 00:00:00', " \
00059                    "`removed_observed` timestamp NOT NULL DEFAULT '0000-00-00 00:00:00', " \
00060                    "PRIMARY KEY (`id`) " \
00061                  ") ENGINE=InnoDB DEFAULT CHARSET=latin1 AUTO_INCREMENT=1;";
00062 
00063     // the success will not return a result for create table
00064     this->query(sql);
00065   }
00066 }
00067 
00068 void SpatialWorldClient::clearAllEntities() const
00069 {
00070   if (this->connected())
00071   {
00072     string sql = "TRUNCATE TABLE `observations`;";
00073     // the success will not return a result for truncate (very fast)
00074     this->query(sql);
00075   }
00076 }
00077 
00078 void SpatialWorldClient::addObservations(const vector<Observation> &observations) const
00079 {
00080   // add each observation
00081   for (size_t i = 0; i < observations.size(); i++)
00082   {
00083     this->addObservation(observations[i]);
00084   }
00085 }
00086 
00087 void SpatialWorldClient::addObservation(const Item &item, const Surface &surface, const Pose &pose) const
00088 {
00089   // create an Observation object and add it
00090   Observation observation(item, surface, pose);
00091   this->addObservation(observation);
00092 }
00093 
00094 void SpatialWorldClient::addObservation(const Observation &observation) const
00095 {
00096   if (this->connected())
00097   {
00098     // to make function calls shorter
00099     const Item item = observation.getItem();
00100     const Surface surface = observation.getSurface();
00101     const Pose pose = observation.getPose();
00102 
00103     // build the SQL statement
00104     stringstream ss;
00105     ss << "INSERT INTO `observations` (`item_name`, `surface_name`, `surface_frame_id`, `x`, `y`, `z`, `theta`, "
00106     << "`time`, `removed_estimate`, `removed_observed`) VALUES ("
00107     << "\"" << item.getName() << "\", "
00108     << "\"" << surface.getName() << "\", "
00109     << "\"" << surface.getFrameID() << "\", "
00110     << pose.getPosition().getX() << ", "
00111     << pose.getPosition().getY() << ", "
00112     << pose.getPosition().getZ() << ", "
00113     << pose.getOrientation().getTheta() << ", "
00114     << "FROM_UNIXTIME(" << observation.getTime().sec << ")" << ", "
00115     << "FROM_UNIXTIME(" << observation.getRemovedEstimate().sec << ")" << ", "
00116     << "FROM_UNIXTIME(" << observation.getRemovedObserved().sec << ")"
00117     << ");";
00118 
00119     // execute the query (no result returned for an insert)
00120     this->query(ss.str());
00121   } else
00122   {
00123     ROS_WARN("Attempted to add an observation when no connection has been made.");
00124   }
00125 }
00126 
00127 void SpatialWorldClient::getObservationsByItemName(const string &item_name,
00128     vector<SpatialWorldObservation> &observations) const
00129 {
00130   // build the SQL where clause and ignore case
00131   string where_clause = "UPPER(item_name)=\"" + boost::to_upper_copy(item_name) + "\"";
00132   this->getObservationsHelper(observations, where_clause, 0, "`time` DESC");
00133 }
00134 
00135 void SpatialWorldClient::getObservationsBySurfaceName(const string &surface_name,
00136     vector<SpatialWorldObservation> &observations) const
00137 {
00138   // build the SQL where clause and ignore case
00139   string where_clause = "UPPER(surface_name)=\"" + boost::to_upper_copy(surface_name) + "\"";
00140   this->getObservationsHelper(observations, where_clause, 0, "`time` DESC");
00141 }
00142 
00143 void SpatialWorldClient::getObservationsByItemAndSurfaceName(const string &item_name, const string &surface_name,
00144     vector<SpatialWorldObservation> &observations) const
00145 {
00146   // build the SQL where clause and ignore case
00147   string where_clause = "((UPPER(item_name)=\"" + boost::to_upper_copy(item_name) + "\") AND (UPPER(surface_name)=\""
00148                         + boost::to_upper_copy(surface_name) + "\"))";
00149   this->getObservationsHelper(observations, where_clause, 0, "`time` DESC");
00150 }
00151 
00152 void SpatialWorldClient::getObservationsBySurfaceFrameID(const string &surface_frame_id,
00153     vector<SpatialWorldObservation> &observations) const
00154 {
00155   // build the SQL where clause
00156   string where_clause = "UPPER(surface_frame_id)=\"" + surface_frame_id + "\"";
00157   this->getObservationsHelper(observations, where_clause, 0, "`time` DESC");
00158 }
00159 
00160 string SpatialWorldClient::getMostFrequentSurfaceName(const Item &item) const
00161 {
00162   return this->getMostFrequentSurfaceName(item.getName());
00163 }
00164 
00165 string SpatialWorldClient::getMostFrequentSurfaceName(const string &item_name) const
00166 {
00167   string surface_name;
00168 
00169   // build the SQL
00170   string sql = "SELECT `surface_name`, COUNT(`surface_name`) AS `count` FROM `observations` WHERE `item_name`=\""
00171                + boost::to_upper_copy(item_name) + "\" GROUP BY `surface_name` ORDER BY `count` DESC LIMIT 1;";
00172   // check for a result
00173   MYSQL_RES *result = this->query(sql);
00174   if (result != NULL)
00175   {
00176     // get the first and only result
00177     surface_name = string(mysql_fetch_row(result)[0]);
00178     mysql_free_result(result);
00179   } else
00180   {
00181     ROS_WARN("No observations found for '%s' -- will return the empty string for most frequent surface name.",
00182              item_name.c_str());
00183   }
00184 
00185   return surface_name;
00186 }
00187 
00188 void SpatialWorldClient::updateObservation(const SpatialWorldObservation &observation) const
00189 {
00190   // create the SQL statement
00191   stringstream ss;
00192   ss << "UPDATE `observations` SET "
00193   << "`id`=" << observation.getID() << ", "
00194   << "`item_name`='" << observation.getItemName() << "', "
00195   << "`surface_name`='" << observation.getSurfaceName() << "', "
00196   << "`surface_frame_id`='" << observation.getSurfaceFrameID() << "', "
00197   << "`x`=" << observation.getPose().getPosition().getX() << ", "
00198   << "`y`=" << observation.getPose().getPosition().getY() << ", "
00199   << "`z`=" << observation.getPose().getPosition().getZ() << ", "
00200   << "`theta`=" << observation.getPose().getOrientation().getTheta() << ", "
00201   << "`time`=FROM_UNIXTIME(" << observation.getTime().sec << "), "
00202   << "`removed_estimate`=FROM_UNIXTIME(" << observation.getRemovedEstimate().sec << "), "
00203   << "`removed_observed`=FROM_UNIXTIME(" << observation.getRemovedObserved().sec << ") "
00204   << "WHERE `id`=" << observation.getID() << ";";
00205   // execute the update (no result returned)
00206   this->query(ss.str());
00207 }
00208 
00209 bool SpatialWorldClient::itemExistsOnSurface(const string &item_name, const string &surface_name) const
00210 {
00211   if (this->connected())
00212   {
00213     // build the SQL where clause
00214     string where_clause = "((UPPER(item_name)=\"" + item_name + "\") AND "
00215                           + "(UPPER(surface_name)=\"" + surface_name + "\") AND "
00216                           + "(`removed_observed`='0000-00-00 00:00:00'))";
00217     vector<SpatialWorldObservation> observations;
00218     // attempt to find the latest observation that meets our criteria
00219     this->getObservationsHelper(observations, where_clause, 1);
00220     // check if we have our match
00221     return !observations.empty();
00222   } else
00223   {
00224     ROS_WARN("Attempted to check if a %s exists on the %s when no connection has been made.", item_name.c_str(),
00225              surface_name.c_str());
00226     return false;
00227   }
00228 }
00229 
00230 bool SpatialWorldClient::itemObservedOnSurface(const world::Item &item, const world::Surface &surface) const
00231 {
00232   this->itemObservedOnSurface(item.getName(), surface.getName());
00233 }
00234 
00235 bool SpatialWorldClient::itemObservedOnSurface(const string &item_name, const string &surface_name) const
00236 {
00237   if (this->connected())
00238   {
00239     // build the SQL where clause
00240     string where_clause = "((UPPER(item_name)=\"" + item_name + "\") AND "
00241                           + "(UPPER(surface_name)=\"" + surface_name + "\"))";
00242     vector<SpatialWorldObservation> observations;
00243     // attempt to find the latest observation that meets our criteria
00244     this->getObservationsHelper(observations, where_clause, 1);
00245     // check if we have our match and if has not been seen leaving
00246     return observations.size() == 1;
00247   } else
00248   {
00249     ROS_WARN("Attempted to check if a %s has have been observed on the %s when no connection has been made.",
00250              item_name.c_str(), surface_name.c_str());
00251     return false;
00252   }
00253 }
00254 
00255 void SpatialWorldClient::markObservationsAsRemoved(const Item &item, const Surface &surface,
00256     const ros::Time &removed_observed)
00257 {
00258   this->markObservationsAsRemoved(item.getName(), surface.getName(), removed_observed);
00259 }
00260 
00261 void SpatialWorldClient::markObservationsAsRemoved(const string &item_name, const string &surface_name,
00262     const ros::Time &removed_observed)
00263 {
00264   if (this->connected())
00265   {
00266     // grab all the entities that have not been marked
00267     string where_clause = "((UPPER(item_name)=\"" + item_name + "\") AND "
00268                           + "(UPPER(surface_name)=\"" + surface_name + "\") AND "
00269                           + "(`removed_observed`='0000-00-00 00:00:00'))";
00270     vector<SpatialWorldObservation> observations;
00271     this->getObservationsHelper(observations, where_clause);
00272 
00273     // check if we have any to mark
00274     if (!observations.empty())
00275     {
00276       // update the observed removed for everything but the first (we will also update the estimated time there)
00277       for (size_t i = 1; i < observations.size(); i++)
00278       {
00279         observations[i].setRemovedObserved(removed_observed);
00280         this->updateObservation(observations[i]);
00281       }
00282 
00283       // create an estimated time the item was removed using a Gaussian distribution
00284       SpatialWorldObservation &persistent = observations.front();
00285       SpatialWorldObservation &latest = observations.back();
00286       double delta = removed_observed.toSec() - latest.getTime().toSec();
00287       double mu = delta / 2.0;
00288       double sigma = (delta - mu) / 3.0;
00289       // normal distribution
00290       boost::normal_distribution<> distribution(mu, sigma);
00291       boost::variate_generator<boost::mt19937 &, boost::normal_distribution<> > generator(random_, distribution);
00292 
00293       // get the random number
00294       double rand = max(0.0, min(delta, generator()));
00295 
00296       // set the estimated and observed time and update
00297       persistent.setRemovedObserved(removed_observed);
00298       persistent.setRemovedEstimate(latest.getTime() + ros::Duration(rand));
00299       this->updateObservation(persistent);
00300     } else
00301     {
00302       ROS_WARN("Attempted to mark the %s on the %s as removed when it was not still on that surface.",
00303                item_name.c_str(), surface_name.c_str());
00304     }
00305   } else
00306   {
00307     ROS_WARN("Attempted to mark the %s on the %s as removed when no connection has been made.",
00308              item_name.c_str(), surface_name.c_str());
00309   }
00310 }
00311 
00312 void SpatialWorldClient::getUniqueSurfaceNames(vector<string> &names) const
00313 {
00314   // build the SQL
00315   string sql = "SELECT DISTINCT `surface_name` FROM `observations` ORDER BY `surface_name`;";
00316   // get the result
00317   MYSQL_RES *result = this->query(sql);
00318   if (result != NULL)
00319   {
00320     // parse each result
00321     MYSQL_ROW entity;
00322     while ((entity = mysql_fetch_row(result)) != NULL)
00323     {
00324       names.push_back(entity[0]);
00325     }
00326     mysql_free_result(result);
00327   }
00328 }
00329 
00330 PersistenceModel SpatialWorldClient::getPersistenceModel(const string &item_name, const string &surface_name) const
00331 {
00332   return this->getPersistenceModel(Item(item_name), Surface(surface_name));
00333 }
00334 
00335 PersistenceModel SpatialWorldClient::getPersistenceModel(const Item &item, const Surface &surface) const
00336 {
00337   // load the models we need
00338   vector<SpatialWorldObservation> observations;
00339   string where_clause = "((UPPER(item_name)=\"" + boost::to_upper_copy(item.getName()) + "\") AND "
00340                         + "(UPPER(surface_name)=\"" + boost::to_upper_copy(surface.getName()) + "\") AND NOT "
00341                         + "(`removed_estimate`='0000-00-00 00:00:00'))";
00342   this->getObservationsHelper(observations, where_clause, 0, "`time` DESC");
00343 
00344   // any observation with a valid estimate is a guess at the time-to-removal
00345   double mu = 0;
00346   for (size_t i = 0; i < observations.size(); i++)
00347   {
00348     // compute and convert to hours
00349     ros::Duration diff = observations[i].getRemovedEstimate() - observations[i].getTime();
00350     mu += diff.toSec() / 3600.0;
00351   }
00352   mu /= observations.size();
00353   // lambda is the reciprocal of the expected value
00354   double lambda = 1.0 / mu;
00355 
00356   return PersistenceModel(item, surface, lambda, observations.size(), observations.back().getTime());
00357 }
00358 
00359 void SpatialWorldClient::getObservationsHelper(vector<SpatialWorldObservation> &observations,
00360     const string &where_clause, const uint32_t limit, const string &order_by) const
00361 {
00362   if (this->connected())
00363   {
00364     // build the SQL
00365     string sql = "SELECT `id`, `item_name`, `surface_name`, `surface_frame_id`, `x`, `y`, `z`, `theta`,  " \
00366                  "UNIX_TIMESTAMP(time), UNIX_TIMESTAMP(removed_estimate), UNIX_TIMESTAMP(removed_observed) "\
00367                  "FROM `observations` WHERE " + where_clause + " ORDER BY " + order_by;
00368     if (limit > 0)
00369     {
00370       stringstream ss;
00371       ss << sql << " LIMIT " << limit;
00372       sql = ss.str();
00373     }
00374     sql += ";";
00375 
00376     MYSQL_RES *result = this->query(sql);
00377     if (result != NULL)
00378     {
00379       // parse each result
00380       MYSQL_ROW entity;
00381       while ((entity = mysql_fetch_row(result)) != NULL)
00382       {
00383         // basic information about the observation
00384         uint32_t id = atol(entity[0]);
00385         string item_name(entity[1]);
00386         string surface_name(entity[2]);
00387         string surface_frame_id(entity[3]);
00388         // position information
00389         Position position(atof(entity[4]), atof(entity[5]), atof(entity[6]));
00390         Orientation orientation(atof(entity[7]));
00391         Pose pose(position, orientation);
00392         // time information
00393         ros::Time time(atol(entity[8]));
00394         ros::Time removed_estimate(atol(entity[9]));
00395         ros::Time removed_observed(atol(entity[10]));
00396 
00397         // create and add
00398         SpatialWorldObservation observation(id, item_name, surface_name, surface_frame_id, pose, time, removed_estimate,
00399                                             removed_observed);
00400         observations.push_back(observation);
00401       }
00402     } else
00403     {
00404       ROS_WARN("Loading observations did not return a result.");
00405     }
00406   } else
00407   {
00408     ROS_WARN("Attempted to load observations for when no connection has been made.");
00409   }
00410 }


worldlib
Author(s): Russell Toris
autogenerated on Thu Jun 6 2019 20:55:36