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 moveit_warehouse::PlanningSceneWorldStorage::PlanningSceneWorldStorage(const std::string &host, const unsigned int port, double wait_seconds) :
00043 MoveItMessageStorage(host, port, wait_seconds)
00044 {
00045 createCollections();
00046 ROS_DEBUG("Connected to MongoDB '%s' on host '%s' port '%u'.", DATABASE_NAME.c_str(), db_host_.c_str(), db_port_);
00047 }
00048
00049 void moveit_warehouse::PlanningSceneWorldStorage::createCollections()
00050 {
00051 planning_scene_world_collection_.reset(new PlanningSceneWorldCollection::element_type(DATABASE_NAME, "planning_scene_worlds", db_host_, db_port_, timeout_));
00052 }
00053
00054 void moveit_warehouse::PlanningSceneWorldStorage::reset()
00055 {
00056 planning_scene_world_collection_.reset();
00057 MoveItMessageStorage::drop(DATABASE_NAME);
00058 createCollections();
00059 }
00060
00061 void moveit_warehouse::PlanningSceneWorldStorage::addPlanningSceneWorld(const moveit_msgs::PlanningSceneWorld &msg, const std::string &name)
00062 {
00063 bool replace = false;
00064 if (hasPlanningSceneWorld(name))
00065 {
00066 removePlanningSceneWorld(name);
00067 replace = true;
00068 }
00069 mongo_ros::Metadata metadata(PLANNING_SCENE_WORLD_ID_NAME, name);
00070 planning_scene_world_collection_->insert(msg, metadata);
00071 ROS_DEBUG("%s planning scene world '%s'", replace ? "Replaced" : "Added", name.c_str());
00072 }
00073
00074 bool moveit_warehouse::PlanningSceneWorldStorage::hasPlanningSceneWorld(const std::string &name) const
00075 {
00076 mongo_ros::Query q(PLANNING_SCENE_WORLD_ID_NAME, name);
00077 std::vector<PlanningSceneWorldWithMetadata> psw = planning_scene_world_collection_->pullAllResults(q, true);
00078 return !psw.empty();
00079 }
00080
00081 void moveit_warehouse::PlanningSceneWorldStorage::getKnownPlanningSceneWorlds(const std::string ®ex, std::vector<std::string> &names) const
00082 {
00083 getKnownPlanningSceneWorlds(names);
00084 filterNames(regex, names);
00085 }
00086
00087 void moveit_warehouse::PlanningSceneWorldStorage::getKnownPlanningSceneWorlds(std::vector<std::string> &names) const
00088 {
00089 names.clear();
00090 mongo_ros::Query q;
00091 std::vector<PlanningSceneWorldWithMetadata> constr = planning_scene_world_collection_->pullAllResults(q, true, PLANNING_SCENE_WORLD_ID_NAME, true);
00092 for (std::size_t i = 0; i < constr.size() ; ++i)
00093 if (constr[i]->metadata.hasField(PLANNING_SCENE_WORLD_ID_NAME.c_str()))
00094 names.push_back(constr[i]->lookupString(PLANNING_SCENE_WORLD_ID_NAME));
00095 }
00096
00097 bool moveit_warehouse::PlanningSceneWorldStorage::getPlanningSceneWorld(PlanningSceneWorldWithMetadata &msg_m, const std::string &name) const
00098 {
00099 mongo_ros::Query q(PLANNING_SCENE_WORLD_ID_NAME, name);
00100 std::vector<PlanningSceneWorldWithMetadata> psw = planning_scene_world_collection_->pullAllResults(q, false);
00101 if (psw.empty())
00102 return false;
00103 else
00104 {
00105 msg_m = psw.front();
00106 return true;
00107 }
00108 }
00109
00110 void moveit_warehouse::PlanningSceneWorldStorage::renamePlanningSceneWorld(const std::string &old_name, const std::string &new_name)
00111 {
00112 mongo_ros::Query q(PLANNING_SCENE_WORLD_ID_NAME, old_name);
00113 mongo_ros::Metadata m(PLANNING_SCENE_WORLD_ID_NAME, new_name);
00114 planning_scene_world_collection_->modifyMetadata(q, m);
00115 ROS_DEBUG("Renamed planning scene world from '%s' to '%s'", old_name.c_str(), new_name.c_str());
00116 }
00117
00118 void moveit_warehouse::PlanningSceneWorldStorage::removePlanningSceneWorld(const std::string &name)
00119 {
00120 mongo_ros::Query q(PLANNING_SCENE_WORLD_ID_NAME, name);
00121 unsigned int rem = planning_scene_world_collection_->removeMessages(q);
00122 ROS_DEBUG("Removed %u PlanningSceneWorld messages (named '%s')", rem, name.c_str());
00123 }