Go to the documentation of this file.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_world_storage.h>
00038
00039 const std::string moveit_warehouse::PlanningSceneWorldStorage::DATABASE_NAME = "moveit_planning_scene_worlds";
00040 const std::string moveit_warehouse::PlanningSceneWorldStorage::PLANNING_SCENE_WORLD_ID_NAME = "world_id";
00041
00042 using warehouse_ros::Metadata;
00043 using warehouse_ros::Query;
00044
00045 moveit_warehouse::PlanningSceneWorldStorage::PlanningSceneWorldStorage(warehouse_ros::DatabaseConnection::Ptr conn)
00046 : MoveItMessageStorage(conn)
00047 {
00048 createCollections();
00049 }
00050
00051 void moveit_warehouse::PlanningSceneWorldStorage::createCollections()
00052 {
00053 planning_scene_world_collection_ =
00054 conn_->openCollectionPtr<moveit_msgs::PlanningSceneWorld>(DATABASE_NAME, "planning_scene_worlds");
00055 }
00056
00057 void moveit_warehouse::PlanningSceneWorldStorage::reset()
00058 {
00059 planning_scene_world_collection_.reset();
00060 conn_->dropDatabase(DATABASE_NAME);
00061 createCollections();
00062 }
00063
00064 void moveit_warehouse::PlanningSceneWorldStorage::addPlanningSceneWorld(const moveit_msgs::PlanningSceneWorld& msg,
00065 const std::string& name)
00066 {
00067 bool replace = false;
00068 if (hasPlanningSceneWorld(name))
00069 {
00070 removePlanningSceneWorld(name);
00071 replace = true;
00072 }
00073 Metadata::Ptr metadata = planning_scene_world_collection_->createMetadata();
00074 metadata->append(PLANNING_SCENE_WORLD_ID_NAME, name);
00075 planning_scene_world_collection_->insert(msg, metadata);
00076 ROS_DEBUG("%s planning scene world '%s'", replace ? "Replaced" : "Added", name.c_str());
00077 }
00078
00079 bool moveit_warehouse::PlanningSceneWorldStorage::hasPlanningSceneWorld(const std::string& name) const
00080 {
00081 Query::Ptr q = planning_scene_world_collection_->createQuery();
00082 q->append(PLANNING_SCENE_WORLD_ID_NAME, name);
00083 std::vector<PlanningSceneWorldWithMetadata> psw = planning_scene_world_collection_->queryList(q, true);
00084 return !psw.empty();
00085 }
00086
00087 void moveit_warehouse::PlanningSceneWorldStorage::getKnownPlanningSceneWorlds(const std::string& regex,
00088 std::vector<std::string>& names) const
00089 {
00090 getKnownPlanningSceneWorlds(names);
00091 filterNames(regex, names);
00092 }
00093
00094 void moveit_warehouse::PlanningSceneWorldStorage::getKnownPlanningSceneWorlds(std::vector<std::string>& names) const
00095 {
00096 names.clear();
00097 Query::Ptr q = planning_scene_world_collection_->createQuery();
00098 std::vector<PlanningSceneWorldWithMetadata> constr =
00099 planning_scene_world_collection_->queryList(q, true, PLANNING_SCENE_WORLD_ID_NAME, true);
00100 for (std::size_t i = 0; i < constr.size(); ++i)
00101 if (constr[i]->lookupField(PLANNING_SCENE_WORLD_ID_NAME))
00102 names.push_back(constr[i]->lookupString(PLANNING_SCENE_WORLD_ID_NAME));
00103 }
00104
00105 bool moveit_warehouse::PlanningSceneWorldStorage::getPlanningSceneWorld(PlanningSceneWorldWithMetadata& msg_m,
00106 const std::string& name) const
00107 {
00108 Query::Ptr q = planning_scene_world_collection_->createQuery();
00109 q->append(PLANNING_SCENE_WORLD_ID_NAME, name);
00110 std::vector<PlanningSceneWorldWithMetadata> psw = planning_scene_world_collection_->queryList(q, false);
00111 if (psw.empty())
00112 return false;
00113 else
00114 {
00115 msg_m = psw.front();
00116 return true;
00117 }
00118 }
00119
00120 void moveit_warehouse::PlanningSceneWorldStorage::renamePlanningSceneWorld(const std::string& old_name,
00121 const std::string& new_name)
00122 {
00123 Query::Ptr q = planning_scene_world_collection_->createQuery();
00124 q->append(PLANNING_SCENE_WORLD_ID_NAME, old_name);
00125 Metadata::Ptr m = planning_scene_world_collection_->createMetadata();
00126 m->append(PLANNING_SCENE_WORLD_ID_NAME, new_name);
00127 planning_scene_world_collection_->modifyMetadata(q, m);
00128 ROS_DEBUG("Renamed planning scene world from '%s' to '%s'", old_name.c_str(), new_name.c_str());
00129 }
00130
00131 void moveit_warehouse::PlanningSceneWorldStorage::removePlanningSceneWorld(const std::string& name)
00132 {
00133 Query::Ptr q = planning_scene_world_collection_->createQuery();
00134 q->append(PLANNING_SCENE_WORLD_ID_NAME, name);
00135 unsigned int rem = planning_scene_world_collection_->removeMessages(q);
00136 ROS_DEBUG("Removed %u PlanningSceneWorld messages (named '%s')", rem, name.c_str());
00137 }