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/constraints_storage.h>
00038
00039 const std::string moveit_warehouse::ConstraintsStorage::DATABASE_NAME = "moveit_constraints";
00040
00041 const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME = "constraints_id";
00042 const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_GROUP_NAME = "group_id";
00043 const std::string moveit_warehouse::ConstraintsStorage::ROBOT_NAME = "robot_id";
00044
00045 moveit_warehouse::ConstraintsStorage::ConstraintsStorage(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::ConstraintsStorage::createCollections()
00053 {
00054 constraints_collection_.reset(new ConstraintsCollection::element_type(DATABASE_NAME, "constraints", db_host_, db_port_, timeout_));
00055 }
00056
00057 void moveit_warehouse::ConstraintsStorage::reset()
00058 {
00059 constraints_collection_.reset();
00060 MoveItMessageStorage::drop(DATABASE_NAME);
00061 createCollections();
00062 }
00063
00064 void moveit_warehouse::ConstraintsStorage::addConstraints(const moveit_msgs::Constraints &msg, const std::string &robot, const std::string &group)
00065 {
00066 bool replace = false;
00067 if (hasConstraints(msg.name, robot, group))
00068 {
00069 removeConstraints(msg.name, robot, group);
00070 replace = true;
00071 }
00072 mongo_ros::Metadata metadata(CONSTRAINTS_ID_NAME, msg.name,
00073 ROBOT_NAME, robot,
00074 CONSTRAINTS_GROUP_NAME, group);
00075 constraints_collection_->insert(msg, metadata);
00076 ROS_DEBUG("%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str());
00077 }
00078
00079 bool moveit_warehouse::ConstraintsStorage::hasConstraints(const std::string &name, const std::string &robot, const std::string &group) const
00080 {
00081 mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00082 if (!robot.empty())
00083 q.append(ROBOT_NAME, robot);
00084 if (!group.empty())
00085 q.append(CONSTRAINTS_GROUP_NAME, group);
00086 std::vector<ConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, true);
00087 return !constr.empty();
00088 }
00089
00090 void moveit_warehouse::ConstraintsStorage::getKnownConstraints(const std::string ®ex, std::vector<std::string> &names, const std::string &robot, const std::string &group) const
00091 {
00092 getKnownConstraints(names, robot, group);
00093 filterNames(regex, names);
00094 }
00095
00096 void moveit_warehouse::ConstraintsStorage::getKnownConstraints(std::vector<std::string> &names, const std::string &robot, const std::string &group) const
00097 {
00098 names.clear();
00099 mongo_ros::Query q;
00100 if (!robot.empty())
00101 q.append(ROBOT_NAME, robot);
00102 if (!group.empty())
00103 q.append(CONSTRAINTS_GROUP_NAME, group);
00104 std::vector<ConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, true, CONSTRAINTS_ID_NAME, true);
00105 for (std::size_t i = 0; i < constr.size() ; ++i)
00106 if (constr[i]->metadata.hasField(CONSTRAINTS_ID_NAME.c_str()))
00107 names.push_back(constr[i]->lookupString(CONSTRAINTS_ID_NAME));
00108 }
00109
00110 bool moveit_warehouse::ConstraintsStorage::getConstraints(ConstraintsWithMetadata &msg_m, const std::string &name, const std::string &robot, const std::string &group) const
00111 {
00112 mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00113 if (!robot.empty())
00114 q.append(ROBOT_NAME, robot);
00115 if (!group.empty())
00116 q.append(CONSTRAINTS_GROUP_NAME, group);
00117 std::vector<ConstraintsWithMetadata> constr = constraints_collection_->pullAllResults(q, false);
00118 if (constr.empty())
00119 return false;
00120 else
00121 {
00122 msg_m = constr.back();
00123
00124 const_cast<moveit_msgs::Constraints*>(static_cast<const moveit_msgs::Constraints*>(msg_m.get()))->name = name;
00125 return true;
00126 }
00127 }
00128
00129 void moveit_warehouse::ConstraintsStorage::renameConstraints(const std::string &old_name, const std::string &new_name, const std::string &robot, const std::string &group)
00130 {
00131 mongo_ros::Query q(CONSTRAINTS_ID_NAME, old_name);
00132 if (!robot.empty())
00133 q.append(ROBOT_NAME, robot);
00134 if (!group.empty())
00135 q.append(CONSTRAINTS_GROUP_NAME, group);
00136 mongo_ros::Metadata m(CONSTRAINTS_ID_NAME, new_name);
00137 constraints_collection_->modifyMetadata(q, m);
00138 ROS_DEBUG("Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
00139 }
00140
00141 void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string &name, const std::string &robot, const std::string &group)
00142 {
00143 mongo_ros::Query q(CONSTRAINTS_ID_NAME, name);
00144 if (!robot.empty())
00145 q.append(ROBOT_NAME, robot);
00146 if (!group.empty())
00147 q.append(CONSTRAINTS_GROUP_NAME, group);
00148 unsigned int rem = constraints_collection_->removeMessages(q);
00149 ROS_DEBUG("Removed %u Constraints messages (named '%s')", rem, name.c_str());
00150 }