00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
00095 if (existing_requests.empty())
00096 return "";
00097
00098
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
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
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 ®ex, 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
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 ®ex, 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 }