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 using warehouse_ros::Metadata;
00046 using warehouse_ros::Query;
00047
00048 moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage(
00049 warehouse_ros::DatabaseConnection::Ptr conn)
00050 : MoveItMessageStorage(conn)
00051 {
00052 createCollections();
00053 }
00054
00055 void moveit_warehouse::TrajectoryConstraintsStorage::createCollections(void)
00056 {
00057 constraints_collection_ =
00058 conn_->openCollectionPtr<moveit_msgs::TrajectoryConstraints>(DATABASE_NAME, "trajectory_constraints");
00059 }
00060
00061 void moveit_warehouse::TrajectoryConstraintsStorage::reset(void)
00062 {
00063 constraints_collection_.reset();
00064 conn_->dropDatabase(DATABASE_NAME);
00065 createCollections();
00066 }
00067
00068 void moveit_warehouse::TrajectoryConstraintsStorage::addTrajectoryConstraints(
00069 const moveit_msgs::TrajectoryConstraints& msg, const std::string& name, const std::string& robot,
00070 const std::string& group)
00071 {
00072 bool replace = false;
00073 if (hasTrajectoryConstraints(name, robot, group))
00074 {
00075 removeTrajectoryConstraints(name, robot, group);
00076 replace = true;
00077 }
00078 Metadata::Ptr metadata = constraints_collection_->createMetadata();
00079 metadata->append(CONSTRAINTS_ID_NAME, name);
00080 metadata->append(ROBOT_NAME, robot);
00081 metadata->append(CONSTRAINTS_GROUP_NAME, group);
00082 constraints_collection_->insert(msg, metadata);
00083 ROS_DEBUG("%s constraints '%s'", replace ? "Replaced" : "Added", name.c_str());
00084 }
00085
00086 bool moveit_warehouse::TrajectoryConstraintsStorage::hasTrajectoryConstraints(const std::string& name,
00087 const std::string& robot,
00088 const std::string& group) const
00089 {
00090 Query::Ptr q = constraints_collection_->createQuery();
00091 q->append(CONSTRAINTS_ID_NAME, name);
00092 if (!robot.empty())
00093 q->append(ROBOT_NAME, robot);
00094 if (!group.empty())
00095 q->append(CONSTRAINTS_GROUP_NAME, group);
00096 std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->queryList(q, true);
00097 return !constr.empty();
00098 }
00099
00100 void moveit_warehouse::TrajectoryConstraintsStorage::getKnownTrajectoryConstraints(const std::string& regex,
00101 std::vector<std::string>& names,
00102 const std::string& robot,
00103 const std::string& group) const
00104 {
00105 getKnownTrajectoryConstraints(names, robot, group);
00106 filterNames(regex, names);
00107 }
00108
00109 void moveit_warehouse::TrajectoryConstraintsStorage::getKnownTrajectoryConstraints(std::vector<std::string>& names,
00110 const std::string& robot,
00111 const std::string& group) const
00112 {
00113 names.clear();
00114 Query::Ptr q = constraints_collection_->createQuery();
00115 if (!robot.empty())
00116 q->append(ROBOT_NAME, robot);
00117 if (!group.empty())
00118 q->append(CONSTRAINTS_GROUP_NAME, group);
00119 std::vector<TrajectoryConstraintsWithMetadata> constr =
00120 constraints_collection_->queryList(q, true, CONSTRAINTS_ID_NAME, true);
00121 for (std::size_t i = 0; i < constr.size(); ++i)
00122 if (constr[i]->lookupField(CONSTRAINTS_ID_NAME))
00123 names.push_back(constr[i]->lookupString(CONSTRAINTS_ID_NAME));
00124 }
00125
00126 bool moveit_warehouse::TrajectoryConstraintsStorage::getTrajectoryConstraints(TrajectoryConstraintsWithMetadata& msg_m,
00127 const std::string& name,
00128 const std::string& robot,
00129 const std::string& group) const
00130 {
00131 Query::Ptr q = constraints_collection_->createQuery();
00132 q->append(CONSTRAINTS_ID_NAME, name);
00133 if (!robot.empty())
00134 q->append(ROBOT_NAME, robot);
00135 if (!group.empty())
00136 q->append(CONSTRAINTS_GROUP_NAME, group);
00137 std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->queryList(q, false);
00138 if (constr.empty())
00139 return false;
00140 else
00141 {
00142 msg_m = constr.back();
00143 return true;
00144 }
00145 }
00146
00147 void moveit_warehouse::TrajectoryConstraintsStorage::renameTrajectoryConstraints(const std::string& old_name,
00148 const std::string& new_name,
00149 const std::string& robot,
00150 const std::string& group)
00151 {
00152 Query::Ptr q = constraints_collection_->createQuery();
00153 q->append(CONSTRAINTS_ID_NAME, old_name);
00154 if (!robot.empty())
00155 q->append(ROBOT_NAME, robot);
00156 if (!group.empty())
00157 q->append(CONSTRAINTS_GROUP_NAME, group);
00158 Metadata::Ptr m = constraints_collection_->createMetadata();
00159 m->append(CONSTRAINTS_ID_NAME, new_name);
00160 constraints_collection_->modifyMetadata(q, m);
00161 ROS_DEBUG("Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
00162 }
00163
00164 void moveit_warehouse::TrajectoryConstraintsStorage::removeTrajectoryConstraints(const std::string& name,
00165 const std::string& robot,
00166 const std::string& group)
00167 {
00168 Query::Ptr q = constraints_collection_->createQuery();
00169 q->append(CONSTRAINTS_ID_NAME, name);
00170 if (!robot.empty())
00171 q->append(ROBOT_NAME, robot);
00172 if (!group.empty())
00173 q->append(CONSTRAINTS_GROUP_NAME, group);
00174 unsigned int rem = constraints_collection_->removeMessages(q);
00175 ROS_DEBUG("Removed %u TrajectoryConstraints messages (named '%s')", rem, name.c_str());
00176 }