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