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