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