planning_scene_storage.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/warehouse/planning_scene_storage.h>
00038 #include <boost/regex.hpp>
00039 
00040 const std::string moveit_warehouse::PlanningSceneStorage::DATABASE_NAME = "moveit_planning_scenes";
00041 
00042 const std::string moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME = "planning_scene_id";
00043 const std::string moveit_warehouse::PlanningSceneStorage::MOTION_PLAN_REQUEST_ID_NAME = "motion_request_id";
00044 
00045 moveit_warehouse::PlanningSceneStorage::PlanningSceneStorage(const std::string &host, const unsigned int port, double wait_seconds) :
00046   MoveItMessageStorage(host, port, wait_seconds)
00047 {
00048   createCollections();
00049   ROS_DEBUG("Connected to MongoDB '%s' on host '%s' port '%u'.", DATABASE_NAME.c_str(), db_host_.c_str(), db_port_);
00050 }
00051 
00052 void moveit_warehouse::PlanningSceneStorage::createCollections()
00053 {
00054   planning_scene_collection_.reset(new PlanningSceneCollection::element_type(DATABASE_NAME, "planning_scene", db_host_, db_port_, timeout_));
00055   motion_plan_request_collection_.reset(new MotionPlanRequestCollection::element_type(DATABASE_NAME, "motion_plan_request", db_host_, db_port_, timeout_));
00056   robot_trajectory_collection_.reset(new RobotTrajectoryCollection::element_type(DATABASE_NAME, "robot_trajectory", db_host_, db_port_, timeout_));
00057 }
00058 
00059 void moveit_warehouse::PlanningSceneStorage::reset()
00060 {
00061   planning_scene_collection_.reset();
00062   motion_plan_request_collection_.reset();
00063   robot_trajectory_collection_.reset();
00064   MoveItMessageStorage::drop(DATABASE_NAME);
00065   createCollections();
00066 }
00067 
00068 void moveit_warehouse::PlanningSceneStorage::addPlanningScene(const moveit_msgs::PlanningScene &scene)
00069 {
00070   bool replace = false;
00071   if (hasPlanningScene(scene.name))
00072   {
00073     removePlanningScene(scene.name);
00074     replace = true;
00075   }
00076   mongo_ros::Metadata metadata(PLANNING_SCENE_ID_NAME, scene.name);
00077   planning_scene_collection_->insert(scene, metadata);
00078   ROS_DEBUG("%s scene '%s'", replace ? "Replaced" : "Added", scene.name.c_str());
00079 }
00080 
00081 bool moveit_warehouse::PlanningSceneStorage::hasPlanningScene(const std::string &name) const
00082 {
00083   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, name);
00084   std::vector<PlanningSceneWithMetadata> planning_scenes = planning_scene_collection_->pullAllResults(q, true);
00085   return !planning_scenes.empty();
00086 }
00087 
00088 std::string moveit_warehouse::PlanningSceneStorage::getMotionPlanRequestName(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name) const
00089 {
00090   // get all existing motion planning requests for this planning scene
00091   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00092   std::vector<MotionPlanRequestWithMetadata> existing_requests = motion_plan_request_collection_->pullAllResults(q, false);
00093 
00094   // if there are no requests stored, we are done
00095   if (existing_requests.empty())
00096     return "";
00097 
00098   // compute the serialization of the message passed as argument
00099   const size_t serial_size_arg = ros::serialization::serializationLength(planning_query);
00100   boost::shared_array<uint8_t> buffer_arg(new uint8_t[serial_size_arg]);
00101   ros::serialization::OStream stream_arg(buffer_arg.get(), serial_size_arg);
00102   ros::serialization::serialize(stream_arg, planning_query);
00103   const void* data_arg = buffer_arg.get();
00104 
00105   for (std::size_t i = 0 ; i < existing_requests.size() ; ++i)
00106   {
00107     const size_t serial_size = ros::serialization::serializationLength(static_cast<const moveit_msgs::MotionPlanRequest&>(*existing_requests[i]));
00108     if (serial_size != serial_size_arg)
00109       continue;
00110     boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
00111     ros::serialization::OStream stream(buffer.get(), serial_size);
00112     ros::serialization::serialize(stream, static_cast<const moveit_msgs::MotionPlanRequest&>(*existing_requests[i]));
00113     const void* data = buffer.get();
00114     if (memcmp(data_arg, data, serial_size) == 0)
00115       // we found the same message twice
00116       return existing_requests[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME);
00117   }
00118   return "";
00119 }
00120 
00121 void moveit_warehouse::PlanningSceneStorage::addPlanningQuery(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name)
00122 {
00123   std::string id = getMotionPlanRequestName(planning_query, scene_name);
00124 
00125   // if we are trying to overwrite, we remove the old query first (if it exists).
00126   if (!query_name.empty() && id.empty())
00127     removePlanningQuery(scene_name, query_name);
00128 
00129   if (id != query_name || id == "")
00130     addNewPlanningRequest(planning_query, scene_name, query_name);
00131 }
00132 
00133 std::string moveit_warehouse::PlanningSceneStorage::addNewPlanningRequest(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name)
00134 {
00135   std::string id = query_name;
00136   if (id.empty())
00137   {
00138     std::set<std::string> used;
00139     mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00140     std::vector<MotionPlanRequestWithMetadata> existing_requests = motion_plan_request_collection_->pullAllResults(q, true);
00141     for (std::size_t i = 0 ; i < existing_requests.size() ; ++i)
00142       used.insert(existing_requests[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME));
00143     std::size_t index = existing_requests.size();
00144     do
00145     {
00146       id = "Motion Plan Request " + boost::lexical_cast<std::string>(index);
00147       index++;
00148     } while (used.find(id) != used.end());
00149   }
00150   mongo_ros::Metadata metadata(PLANNING_SCENE_ID_NAME, scene_name,
00151                                MOTION_PLAN_REQUEST_ID_NAME, id);
00152   motion_plan_request_collection_->insert(planning_query, metadata);
00153   ROS_DEBUG("Saved planning query '%s' for scene '%s'", id.c_str(), scene_name.c_str());
00154   return id;
00155 }
00156 
00157 void moveit_warehouse::PlanningSceneStorage::addPlanningResult(const moveit_msgs::MotionPlanRequest &planning_query, const moveit_msgs::RobotTrajectory &result, const std::string &scene_name)
00158 {
00159   std::string id = getMotionPlanRequestName(planning_query, scene_name);
00160   if (id.empty())
00161     id = addNewPlanningRequest(planning_query, scene_name, "");
00162   mongo_ros::Metadata metadata(PLANNING_SCENE_ID_NAME, scene_name,
00163                                MOTION_PLAN_REQUEST_ID_NAME, id);
00164   robot_trajectory_collection_->insert(result, metadata);
00165 }
00166 
00167 void moveit_warehouse::PlanningSceneStorage::getPlanningSceneNames(std::vector<std::string> &names) const
00168 {
00169   names.clear();
00170   mongo_ros::Query q;
00171   std::vector<PlanningSceneWithMetadata> planning_scenes = planning_scene_collection_->pullAllResults(q, true, PLANNING_SCENE_ID_NAME, true);
00172   for (std::size_t i = 0; i < planning_scenes.size() ; ++i)
00173     if (planning_scenes[i]->metadata.hasField(PLANNING_SCENE_ID_NAME.c_str()))
00174       names.push_back(planning_scenes[i]->lookupString(PLANNING_SCENE_ID_NAME));
00175 }
00176 
00177 void moveit_warehouse::PlanningSceneStorage::getPlanningSceneNames(const std::string &regex, std::vector<std::string> &names) const
00178 {
00179   getPlanningSceneNames(names);
00180   filterNames(regex, names);
00181 }
00182 
00183 bool moveit_warehouse::PlanningSceneStorage::getPlanningSceneWorld(moveit_msgs::PlanningSceneWorld &world, const std::string &scene_name) const
00184 {
00185   PlanningSceneWithMetadata scene_m;
00186   if (getPlanningScene(scene_m, scene_name))
00187   {
00188     world = scene_m->world;
00189     return true;
00190   }
00191   else
00192     return false;
00193 }
00194 
00195 bool moveit_warehouse::PlanningSceneStorage::getPlanningScene(PlanningSceneWithMetadata &scene_m, const std::string &scene_name) const
00196 {
00197   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00198   std::vector<PlanningSceneWithMetadata> planning_scenes = planning_scene_collection_->pullAllResults(q, false);
00199   if (planning_scenes.empty())
00200   {
00201     ROS_WARN("Planning scene '%s' was not found in the database", scene_name.c_str());
00202     return false;
00203   }
00204   scene_m = planning_scenes.back();
00205   // in case the scene was renamed, the name in the message may be out of date
00206   const_cast<moveit_msgs::PlanningScene*>(static_cast<const moveit_msgs::PlanningScene*>(scene_m.get()))->name = scene_name;
00207   return true;
00208 }
00209 
00210 bool moveit_warehouse::PlanningSceneStorage::getPlanningQuery(MotionPlanRequestWithMetadata &query_m, const std::string &scene_name, const std::string &query_name)
00211 {
00212   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00213   q.append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
00214   std::vector<MotionPlanRequestWithMetadata> planning_queries = motion_plan_request_collection_->pullAllResults(q, false);
00215   if (planning_queries.empty())
00216   {
00217     ROS_ERROR("Planning query '%s' not found for scene '%s'", query_name.c_str(), scene_name.c_str());
00218     return false;
00219   }
00220   else
00221   {
00222     query_m = planning_queries.front();
00223     return true;
00224   }
00225 }
00226 
00227 void moveit_warehouse::PlanningSceneStorage::getPlanningQueries(std::vector<MotionPlanRequestWithMetadata> &planning_queries, const std::string &scene_name) const
00228 {
00229   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00230   planning_queries = motion_plan_request_collection_->pullAllResults(q, false);
00231 }
00232 
00233 void moveit_warehouse::PlanningSceneStorage::getPlanningQueriesNames(std::vector<std::string> &query_names, const std::string &scene_name) const
00234 {
00235   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00236   std::vector<MotionPlanRequestWithMetadata> planning_queries = motion_plan_request_collection_->pullAllResults(q, true);
00237   query_names.clear();
00238   for (std::size_t i = 0 ; i < planning_queries.size() ; ++i)
00239     if (planning_queries[i]->metadata.hasField(MOTION_PLAN_REQUEST_ID_NAME.c_str()))
00240       query_names.push_back(planning_queries[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME));
00241 }
00242 
00243 void moveit_warehouse::PlanningSceneStorage::getPlanningQueriesNames(const std::string &regex, std::vector<std::string> &query_names, const std::string &scene_name) const
00244 {
00245   getPlanningQueriesNames(query_names, scene_name);
00246 
00247   if (!regex.empty())
00248   {
00249     std::vector<std::string> fnames;
00250     boost::regex r(regex);
00251     for (std::size_t i = 0; i < query_names.size() ; ++i)
00252     {
00253       boost::cmatch match;
00254       if (boost::regex_match(query_names[i].c_str(), match, r))
00255       {
00256         fnames.push_back(query_names[i]);
00257       }
00258     }
00259     query_names.swap(fnames);
00260   }
00261 }
00262 
00263 void moveit_warehouse::PlanningSceneStorage::getPlanningQueries(std::vector<MotionPlanRequestWithMetadata> &planning_queries, std::vector<std::string> &query_names, const std::string &scene_name) const
00264 {
00265   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00266   planning_queries = motion_plan_request_collection_->pullAllResults(q, false);
00267   query_names.resize(planning_queries.size());
00268   for (std::size_t i = 0 ; i < planning_queries.size() ; ++i)
00269     if (planning_queries[i]->metadata.hasField(MOTION_PLAN_REQUEST_ID_NAME.c_str()))
00270       query_names[i] = planning_queries[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME);
00271     else
00272       query_names[i].clear();
00273 }
00274 
00275 void moveit_warehouse::PlanningSceneStorage::getPlanningResults(std::vector<RobotTrajectoryWithMetadata> &planning_results,
00276                                 const std::string &scene_name, const moveit_msgs::MotionPlanRequest &planning_query) const
00277 {
00278   std::string id = getMotionPlanRequestName(planning_query, scene_name);
00279   if (id.empty())
00280     planning_results.clear();
00281   else
00282     getPlanningResults(planning_results, id, scene_name);
00283 }
00284 
00285 void moveit_warehouse::PlanningSceneStorage::getPlanningResults(std::vector<RobotTrajectoryWithMetadata> &planning_results,
00286                                 const std::string &scene_name, const std::string &planning_query) const
00287 {
00288   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00289   q.append(MOTION_PLAN_REQUEST_ID_NAME, planning_query);
00290   planning_results = robot_trajectory_collection_->pullAllResults(q, false);
00291 }
00292 
00293 bool moveit_warehouse::PlanningSceneStorage::hasPlanningQuery(const std::string &scene_name, const std::string &query_name) const
00294 {
00295   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00296   q.append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
00297   std::vector<MotionPlanRequestWithMetadata> queries = motion_plan_request_collection_->pullAllResults(q, true);
00298   return !queries.empty();
00299 }
00300 
00301 void moveit_warehouse::PlanningSceneStorage::renamePlanningScene(const std::string &old_scene_name, const std::string &new_scene_name)
00302 {
00303   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, old_scene_name);
00304   mongo_ros::Metadata m(PLANNING_SCENE_ID_NAME, new_scene_name);
00305   planning_scene_collection_->modifyMetadata(q, m);
00306   ROS_DEBUG("Renamed planning scene from '%s' to '%s'", old_scene_name.c_str(), new_scene_name.c_str());
00307 }
00308 
00309 void moveit_warehouse::PlanningSceneStorage::renamePlanningQuery(const std::string &scene_name, const std::string &old_query_name, const std::string &new_query_name)
00310 {
00311   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00312   q.append(MOTION_PLAN_REQUEST_ID_NAME, old_query_name);
00313   mongo_ros::Metadata m(MOTION_PLAN_REQUEST_ID_NAME, new_query_name);
00314   motion_plan_request_collection_->modifyMetadata(q, m);
00315   ROS_DEBUG("Renamed planning query for scene '%s' from '%s' to '%s'", scene_name.c_str(), old_query_name.c_str(), new_query_name.c_str());
00316 }
00317 
00318 void moveit_warehouse::PlanningSceneStorage::removePlanningScene(const std::string &scene_name)
00319 {
00320   removePlanningQueries(scene_name);
00321   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00322   unsigned int rem = planning_scene_collection_->removeMessages(q);
00323   ROS_DEBUG("Removed %u PlanningScene messages (named '%s')", rem, scene_name.c_str());
00324 }
00325 
00326 void moveit_warehouse::PlanningSceneStorage::removePlanningQueries(const std::string &scene_name)
00327 {
00328   removePlanningResults(scene_name);
00329   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00330   unsigned int rem = motion_plan_request_collection_->removeMessages(q);
00331   ROS_DEBUG("Removed %u MotionPlanRequest messages for scene '%s'", rem, scene_name.c_str());
00332 }
00333 
00334 void moveit_warehouse::PlanningSceneStorage::removePlanningQuery(const std::string &scene_name, const std::string &query_name)
00335 {
00336   removePlanningResults(scene_name, query_name);
00337   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00338   q.append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
00339   unsigned int rem = motion_plan_request_collection_->removeMessages(q);
00340   ROS_DEBUG("Removed %u MotionPlanRequest messages for scene '%s', query '%s'", rem, scene_name.c_str(), query_name.c_str());
00341 }
00342 
00343 void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::string &scene_name)
00344 {
00345   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00346   unsigned int rem = robot_trajectory_collection_->removeMessages(q);
00347   ROS_DEBUG("Removed %u RobotTrajectory messages for scene '%s'", rem, scene_name.c_str());
00348 }
00349 
00350 void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::string &scene_name, const std::string &query_name)
00351 {
00352   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00353   q.append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
00354   unsigned int rem = robot_trajectory_collection_->removeMessages(q);
00355   ROS_DEBUG("Removed %u RobotTrajectory messages for scene '%s', query '%s'", rem, scene_name.c_str(), query_name.c_str());
00356 }


warehouse
Author(s): Ioan Sucan
autogenerated on Wed Aug 26 2015 12:43:45