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,
00046                                                              double wait_seconds)
00047   : MoveItMessageStorage(host, port, wait_seconds)
00048 {
00049   createCollections();
00050   ROS_DEBUG("Connected to MongoDB '%s' on host '%s' port '%u'.", DATABASE_NAME.c_str(), db_host_.c_str(), db_port_);
00051 }
00052 
00053 void moveit_warehouse::PlanningSceneStorage::createCollections()
00054 {
00055   planning_scene_collection_.reset(
00056       new PlanningSceneCollection::element_type(DATABASE_NAME, "planning_scene", db_host_, db_port_, timeout_));
00057   motion_plan_request_collection_.reset(new MotionPlanRequestCollection::element_type(
00058       DATABASE_NAME, "motion_plan_request", db_host_, db_port_, timeout_));
00059   robot_trajectory_collection_.reset(
00060       new RobotTrajectoryCollection::element_type(DATABASE_NAME, "robot_trajectory", db_host_, db_port_, timeout_));
00061 }
00062 
00063 void moveit_warehouse::PlanningSceneStorage::reset()
00064 {
00065   planning_scene_collection_.reset();
00066   motion_plan_request_collection_.reset();
00067   robot_trajectory_collection_.reset();
00068   MoveItMessageStorage::drop(DATABASE_NAME);
00069   createCollections();
00070 }
00071 
00072 void moveit_warehouse::PlanningSceneStorage::addPlanningScene(const moveit_msgs::PlanningScene& scene)
00073 {
00074   bool replace = false;
00075   if (hasPlanningScene(scene.name))
00076   {
00077     removePlanningScene(scene.name);
00078     replace = true;
00079   }
00080   mongo_ros::Metadata metadata(PLANNING_SCENE_ID_NAME, scene.name);
00081   planning_scene_collection_->insert(scene, metadata);
00082   ROS_DEBUG("%s scene '%s'", replace ? "Replaced" : "Added", scene.name.c_str());
00083 }
00084 
00085 bool moveit_warehouse::PlanningSceneStorage::hasPlanningScene(const std::string& name) const
00086 {
00087   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, name);
00088   std::vector<PlanningSceneWithMetadata> planning_scenes = planning_scene_collection_->pullAllResults(q, true);
00089   return !planning_scenes.empty();
00090 }
00091 
00092 std::string moveit_warehouse::PlanningSceneStorage::getMotionPlanRequestName(
00093     const moveit_msgs::MotionPlanRequest& planning_query, const std::string& scene_name) const
00094 {
00095   // get all existing motion planning requests for this planning scene
00096   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00097   std::vector<MotionPlanRequestWithMetadata> existing_requests =
00098       motion_plan_request_collection_->pullAllResults(q, false);
00099 
00100   // if there are no requests stored, we are done
00101   if (existing_requests.empty())
00102     return "";
00103 
00104   // compute the serialization of the message passed as argument
00105   const size_t serial_size_arg = ros::serialization::serializationLength(planning_query);
00106   boost::shared_array<uint8_t> buffer_arg(new uint8_t[serial_size_arg]);
00107   ros::serialization::OStream stream_arg(buffer_arg.get(), serial_size_arg);
00108   ros::serialization::serialize(stream_arg, planning_query);
00109   const void* data_arg = buffer_arg.get();
00110 
00111   for (std::size_t i = 0; i < existing_requests.size(); ++i)
00112   {
00113     const size_t serial_size = ros::serialization::serializationLength(
00114         static_cast<const moveit_msgs::MotionPlanRequest&>(*existing_requests[i]));
00115     if (serial_size != serial_size_arg)
00116       continue;
00117     boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
00118     ros::serialization::OStream stream(buffer.get(), serial_size);
00119     ros::serialization::serialize(stream, static_cast<const moveit_msgs::MotionPlanRequest&>(*existing_requests[i]));
00120     const void* data = buffer.get();
00121     if (memcmp(data_arg, data, serial_size) == 0)
00122       // we found the same message twice
00123       return existing_requests[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME);
00124   }
00125   return "";
00126 }
00127 
00128 void moveit_warehouse::PlanningSceneStorage::addPlanningQuery(const moveit_msgs::MotionPlanRequest& planning_query,
00129                                                               const std::string& scene_name,
00130                                                               const std::string& query_name)
00131 {
00132   std::string id = getMotionPlanRequestName(planning_query, scene_name);
00133 
00134   // if we are trying to overwrite, we remove the old query first (if it exists).
00135   if (!query_name.empty() && id.empty())
00136     removePlanningQuery(scene_name, query_name);
00137 
00138   if (id != query_name || id == "")
00139     addNewPlanningRequest(planning_query, scene_name, query_name);
00140 }
00141 
00142 std::string moveit_warehouse::PlanningSceneStorage::addNewPlanningRequest(
00143     const moveit_msgs::MotionPlanRequest& planning_query, const std::string& scene_name, const std::string& query_name)
00144 {
00145   std::string id = query_name;
00146   if (id.empty())
00147   {
00148     std::set<std::string> used;
00149     mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00150     std::vector<MotionPlanRequestWithMetadata> existing_requests =
00151         motion_plan_request_collection_->pullAllResults(q, true);
00152     for (std::size_t i = 0; i < existing_requests.size(); ++i)
00153       used.insert(existing_requests[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME));
00154     std::size_t index = existing_requests.size();
00155     do
00156     {
00157       id = "Motion Plan Request " + boost::lexical_cast<std::string>(index);
00158       index++;
00159     } while (used.find(id) != used.end());
00160   }
00161   mongo_ros::Metadata metadata(PLANNING_SCENE_ID_NAME, scene_name, MOTION_PLAN_REQUEST_ID_NAME, id);
00162   motion_plan_request_collection_->insert(planning_query, metadata);
00163   ROS_DEBUG("Saved planning query '%s' for scene '%s'", id.c_str(), scene_name.c_str());
00164   return id;
00165 }
00166 
00167 void moveit_warehouse::PlanningSceneStorage::addPlanningResult(const moveit_msgs::MotionPlanRequest& planning_query,
00168                                                                const moveit_msgs::RobotTrajectory& result,
00169                                                                const std::string& scene_name)
00170 {
00171   std::string id = getMotionPlanRequestName(planning_query, scene_name);
00172   if (id.empty())
00173     id = addNewPlanningRequest(planning_query, scene_name, "");
00174   mongo_ros::Metadata metadata(PLANNING_SCENE_ID_NAME, scene_name, MOTION_PLAN_REQUEST_ID_NAME, id);
00175   robot_trajectory_collection_->insert(result, metadata);
00176 }
00177 
00178 void moveit_warehouse::PlanningSceneStorage::getPlanningSceneNames(std::vector<std::string>& names) const
00179 {
00180   names.clear();
00181   mongo_ros::Query q;
00182   std::vector<PlanningSceneWithMetadata> planning_scenes =
00183       planning_scene_collection_->pullAllResults(q, true, PLANNING_SCENE_ID_NAME, true);
00184   for (std::size_t i = 0; i < planning_scenes.size(); ++i)
00185     if (planning_scenes[i]->metadata.hasField(PLANNING_SCENE_ID_NAME.c_str()))
00186       names.push_back(planning_scenes[i]->lookupString(PLANNING_SCENE_ID_NAME));
00187 }
00188 
00189 void moveit_warehouse::PlanningSceneStorage::getPlanningSceneNames(const std::string& regex,
00190                                                                    std::vector<std::string>& names) const
00191 {
00192   getPlanningSceneNames(names);
00193   filterNames(regex, names);
00194 }
00195 
00196 bool moveit_warehouse::PlanningSceneStorage::getPlanningSceneWorld(moveit_msgs::PlanningSceneWorld& world,
00197                                                                    const std::string& scene_name) const
00198 {
00199   PlanningSceneWithMetadata scene_m;
00200   if (getPlanningScene(scene_m, scene_name))
00201   {
00202     world = scene_m->world;
00203     return true;
00204   }
00205   else
00206     return false;
00207 }
00208 
00209 bool moveit_warehouse::PlanningSceneStorage::getPlanningScene(PlanningSceneWithMetadata& scene_m,
00210                                                               const std::string& scene_name) const
00211 {
00212   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00213   std::vector<PlanningSceneWithMetadata> planning_scenes = planning_scene_collection_->pullAllResults(q, false);
00214   if (planning_scenes.empty())
00215   {
00216     ROS_WARN("Planning scene '%s' was not found in the database", scene_name.c_str());
00217     return false;
00218   }
00219   scene_m = planning_scenes.back();
00220   // in case the scene was renamed, the name in the message may be out of date
00221   const_cast<moveit_msgs::PlanningScene*>(static_cast<const moveit_msgs::PlanningScene*>(scene_m.get()))->name =
00222       scene_name;
00223   return true;
00224 }
00225 
00226 bool moveit_warehouse::PlanningSceneStorage::getPlanningQuery(MotionPlanRequestWithMetadata& query_m,
00227                                                               const std::string& scene_name,
00228                                                               const std::string& query_name)
00229 {
00230   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00231   q.append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
00232   std::vector<MotionPlanRequestWithMetadata> planning_queries =
00233       motion_plan_request_collection_->pullAllResults(q, false);
00234   if (planning_queries.empty())
00235   {
00236     ROS_ERROR("Planning query '%s' not found for scene '%s'", query_name.c_str(), scene_name.c_str());
00237     return false;
00238   }
00239   else
00240   {
00241     query_m = planning_queries.front();
00242     return true;
00243   }
00244 }
00245 
00246 void moveit_warehouse::PlanningSceneStorage::getPlanningQueries(
00247     std::vector<MotionPlanRequestWithMetadata>& planning_queries, const std::string& scene_name) const
00248 {
00249   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00250   planning_queries = motion_plan_request_collection_->pullAllResults(q, false);
00251 }
00252 
00253 void moveit_warehouse::PlanningSceneStorage::getPlanningQueriesNames(std::vector<std::string>& query_names,
00254                                                                      const std::string& scene_name) const
00255 {
00256   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00257   std::vector<MotionPlanRequestWithMetadata> planning_queries =
00258       motion_plan_request_collection_->pullAllResults(q, true);
00259   query_names.clear();
00260   for (std::size_t i = 0; i < planning_queries.size(); ++i)
00261     if (planning_queries[i]->metadata.hasField(MOTION_PLAN_REQUEST_ID_NAME.c_str()))
00262       query_names.push_back(planning_queries[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME));
00263 }
00264 
00265 void moveit_warehouse::PlanningSceneStorage::getPlanningQueriesNames(const std::string& regex,
00266                                                                      std::vector<std::string>& query_names,
00267                                                                      const std::string& scene_name) const
00268 {
00269   getPlanningQueriesNames(query_names, scene_name);
00270 
00271   if (!regex.empty())
00272   {
00273     std::vector<std::string> fnames;
00274     boost::regex r(regex);
00275     for (std::size_t i = 0; i < query_names.size(); ++i)
00276     {
00277       boost::cmatch match;
00278       if (boost::regex_match(query_names[i].c_str(), match, r))
00279       {
00280         fnames.push_back(query_names[i]);
00281       }
00282     }
00283     query_names.swap(fnames);
00284   }
00285 }
00286 
00287 void moveit_warehouse::PlanningSceneStorage::getPlanningQueries(
00288     std::vector<MotionPlanRequestWithMetadata>& planning_queries, std::vector<std::string>& query_names,
00289     const std::string& scene_name) const
00290 {
00291   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00292   planning_queries = motion_plan_request_collection_->pullAllResults(q, false);
00293   query_names.resize(planning_queries.size());
00294   for (std::size_t i = 0; i < planning_queries.size(); ++i)
00295     if (planning_queries[i]->metadata.hasField(MOTION_PLAN_REQUEST_ID_NAME.c_str()))
00296       query_names[i] = planning_queries[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME);
00297     else
00298       query_names[i].clear();
00299 }
00300 
00301 void moveit_warehouse::PlanningSceneStorage::getPlanningResults(
00302     std::vector<RobotTrajectoryWithMetadata>& planning_results, const std::string& scene_name,
00303     const moveit_msgs::MotionPlanRequest& planning_query) const
00304 {
00305   std::string id = getMotionPlanRequestName(planning_query, scene_name);
00306   if (id.empty())
00307     planning_results.clear();
00308   else
00309     getPlanningResults(planning_results, id, scene_name);
00310 }
00311 
00312 void moveit_warehouse::PlanningSceneStorage::getPlanningResults(
00313     std::vector<RobotTrajectoryWithMetadata>& planning_results, const std::string& scene_name,
00314     const std::string& planning_query) const
00315 {
00316   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00317   q.append(MOTION_PLAN_REQUEST_ID_NAME, planning_query);
00318   planning_results = robot_trajectory_collection_->pullAllResults(q, false);
00319 }
00320 
00321 bool moveit_warehouse::PlanningSceneStorage::hasPlanningQuery(const std::string& scene_name,
00322                                                               const std::string& query_name) const
00323 {
00324   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00325   q.append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
00326   std::vector<MotionPlanRequestWithMetadata> queries = motion_plan_request_collection_->pullAllResults(q, true);
00327   return !queries.empty();
00328 }
00329 
00330 void moveit_warehouse::PlanningSceneStorage::renamePlanningScene(const std::string& old_scene_name,
00331                                                                  const std::string& new_scene_name)
00332 {
00333   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, old_scene_name);
00334   mongo_ros::Metadata m(PLANNING_SCENE_ID_NAME, new_scene_name);
00335   planning_scene_collection_->modifyMetadata(q, m);
00336   ROS_DEBUG("Renamed planning scene from '%s' to '%s'", old_scene_name.c_str(), new_scene_name.c_str());
00337 }
00338 
00339 void moveit_warehouse::PlanningSceneStorage::renamePlanningQuery(const std::string& scene_name,
00340                                                                  const std::string& old_query_name,
00341                                                                  const std::string& new_query_name)
00342 {
00343   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00344   q.append(MOTION_PLAN_REQUEST_ID_NAME, old_query_name);
00345   mongo_ros::Metadata m(MOTION_PLAN_REQUEST_ID_NAME, new_query_name);
00346   motion_plan_request_collection_->modifyMetadata(q, m);
00347   ROS_DEBUG("Renamed planning query for scene '%s' from '%s' to '%s'", scene_name.c_str(), old_query_name.c_str(),
00348             new_query_name.c_str());
00349 }
00350 
00351 void moveit_warehouse::PlanningSceneStorage::removePlanningScene(const std::string& scene_name)
00352 {
00353   removePlanningQueries(scene_name);
00354   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00355   unsigned int rem = planning_scene_collection_->removeMessages(q);
00356   ROS_DEBUG("Removed %u PlanningScene messages (named '%s')", rem, scene_name.c_str());
00357 }
00358 
00359 void moveit_warehouse::PlanningSceneStorage::removePlanningQueries(const std::string& scene_name)
00360 {
00361   removePlanningResults(scene_name);
00362   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00363   unsigned int rem = motion_plan_request_collection_->removeMessages(q);
00364   ROS_DEBUG("Removed %u MotionPlanRequest messages for scene '%s'", rem, scene_name.c_str());
00365 }
00366 
00367 void moveit_warehouse::PlanningSceneStorage::removePlanningQuery(const std::string& scene_name,
00368                                                                  const std::string& query_name)
00369 {
00370   removePlanningResults(scene_name, query_name);
00371   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00372   q.append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
00373   unsigned int rem = motion_plan_request_collection_->removeMessages(q);
00374   ROS_DEBUG("Removed %u MotionPlanRequest messages for scene '%s', query '%s'", rem, scene_name.c_str(),
00375             query_name.c_str());
00376 }
00377 
00378 void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::string& scene_name)
00379 {
00380   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00381   unsigned int rem = robot_trajectory_collection_->removeMessages(q);
00382   ROS_DEBUG("Removed %u RobotTrajectory messages for scene '%s'", rem, scene_name.c_str());
00383 }
00384 
00385 void moveit_warehouse::PlanningSceneStorage::removePlanningResults(const std::string& scene_name,
00386                                                                    const std::string& query_name)
00387 {
00388   mongo_ros::Query q(PLANNING_SCENE_ID_NAME, scene_name);
00389   q.append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
00390   unsigned int rem = robot_trajectory_collection_->removeMessages(q);
00391   ROS_DEBUG("Removed %u RobotTrajectory messages for scene '%s', query '%s'", rem, scene_name.c_str(),
00392             query_name.c_str());
00393 }


warehouse
Author(s): Ioan Sucan
autogenerated on Mon Apr 23 2018 10:25:48