00001
00011
00012 #include "worldlib/remote/SpatialWorldClient.h"
00013
00014
00015 #include <ros/ros.h>
00016
00017
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
00039 this->createTable();
00040 return success;
00041 }
00042
00043 void SpatialWorldClient::createTable()
00044 {
00045 if (this->connected())
00046 {
00047
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
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
00074 this->query(sql);
00075 }
00076 }
00077
00078 void SpatialWorldClient::addObservations(const vector<Observation> &observations) const
00079 {
00080
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
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
00099 const Item item = observation.getItem();
00100 const Surface surface = observation.getSurface();
00101 const Pose pose = observation.getPose();
00102
00103
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
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
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
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
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
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
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
00173 MYSQL_RES *result = this->query(sql);
00174 if (result != NULL)
00175 {
00176
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
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
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
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
00219 this->getObservationsHelper(observations, where_clause, 1);
00220
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
00240 string where_clause = "((UPPER(item_name)=\"" + item_name + "\") AND "
00241 + "(UPPER(surface_name)=\"" + surface_name + "\"))";
00242 vector<SpatialWorldObservation> observations;
00243
00244 this->getObservationsHelper(observations, where_clause, 1);
00245
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
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
00274 if (!observations.empty())
00275 {
00276
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
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
00290 boost::normal_distribution<> distribution(mu, sigma);
00291 boost::variate_generator<boost::mt19937 &, boost::normal_distribution<> > generator(random_, distribution);
00292
00293
00294 double rand = max(0.0, min(delta, generator()));
00295
00296
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
00315 string sql = "SELECT DISTINCT `surface_name` FROM `observations` ORDER BY `surface_name`;";
00316
00317 MYSQL_RES *result = this->query(sql);
00318 if (result != NULL)
00319 {
00320
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
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
00345 double mu = 0;
00346 for (size_t i = 0; i < observations.size(); i++)
00347 {
00348
00349 ros::Duration diff = observations[i].getRemovedEstimate() - observations[i].getTime();
00350 mu += diff.toSec() / 3600.0;
00351 }
00352 mu /= observations.size();
00353
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
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
00380 MYSQL_ROW entity;
00381 while ((entity = mysql_fetch_row(result)) != NULL)
00382 {
00383
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
00389 Position position(atof(entity[4]), atof(entity[5]), atof(entity[6]));
00390 Orientation orientation(atof(entity[7]));
00391 Pose pose(position, orientation);
00392
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
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 }