constraints_storage.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
38 
39 const std::string moveit_warehouse::ConstraintsStorage::DATABASE_NAME = "moveit_constraints";
40 
41 const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME = "constraints_id";
43 const std::string moveit_warehouse::ConstraintsStorage::ROBOT_NAME = "robot_id";
44 
47 
49  : MoveItMessageStorage(conn)
50 {
52 }
53 
55 {
56  constraints_collection_ = conn_->openCollectionPtr<moveit_msgs::Constraints>(DATABASE_NAME, "constraints");
57 }
58 
60 {
62  conn_->dropDatabase(DATABASE_NAME);
64 }
65 
66 void moveit_warehouse::ConstraintsStorage::addConstraints(const moveit_msgs::Constraints& msg, const std::string& robot,
67  const std::string& group)
68 {
69  bool replace = false;
70  if (hasConstraints(msg.name, robot, group))
71  {
72  removeConstraints(msg.name, robot, group);
73  replace = true;
74  }
75  Metadata::Ptr metadata = constraints_collection_->createMetadata();
76  metadata->append(CONSTRAINTS_ID_NAME, msg.name);
77  metadata->append(ROBOT_NAME, robot);
78  metadata->append(CONSTRAINTS_GROUP_NAME, group);
79  constraints_collection_->insert(msg, metadata);
80  ROS_DEBUG("%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str());
81 }
82 
83 bool moveit_warehouse::ConstraintsStorage::hasConstraints(const std::string& name, const std::string& robot,
84  const std::string& group) const
85 {
86  Query::Ptr q = constraints_collection_->createQuery();
87  q->append(CONSTRAINTS_ID_NAME, name);
88  if (!robot.empty())
89  q->append(ROBOT_NAME, robot);
90  if (!group.empty())
91  q->append(CONSTRAINTS_GROUP_NAME, group);
92  std::vector<ConstraintsWithMetadata> constr = constraints_collection_->queryList(q, true);
93  return !constr.empty();
94 }
95 
97  std::vector<std::string>& names,
98  const std::string& robot, const std::string& group) const
99 {
100  getKnownConstraints(names, robot, group);
101  filterNames(regex, names);
102 }
103 
105  const std::string& robot, const std::string& group) const
106 {
107  names.clear();
108  Query::Ptr q = constraints_collection_->createQuery();
109  if (!robot.empty())
110  q->append(ROBOT_NAME, robot);
111  if (!group.empty())
112  q->append(CONSTRAINTS_GROUP_NAME, group);
113  std::vector<ConstraintsWithMetadata> constr = constraints_collection_->queryList(q, true, CONSTRAINTS_ID_NAME, true);
114  for (std::size_t i = 0; i < constr.size(); ++i)
115  if (constr[i]->lookupField(CONSTRAINTS_ID_NAME))
116  names.push_back(constr[i]->lookupString(CONSTRAINTS_ID_NAME));
117 }
118 
120  const std::string& robot, const std::string& group) const
121 {
122  Query::Ptr q = constraints_collection_->createQuery();
123  q->append(CONSTRAINTS_ID_NAME, name);
124  if (!robot.empty())
125  q->append(ROBOT_NAME, robot);
126  if (!group.empty())
127  q->append(CONSTRAINTS_GROUP_NAME, group);
128  std::vector<ConstraintsWithMetadata> constr = constraints_collection_->queryList(q, false);
129  if (constr.empty())
130  return false;
131  else
132  {
133  msg_m = constr.back();
134  // in case the constraints were renamed, the name in the message may be out of date
135  const_cast<moveit_msgs::Constraints*>(static_cast<const moveit_msgs::Constraints*>(msg_m.get()))->name = name;
136  return true;
137  }
138 }
139 
140 void moveit_warehouse::ConstraintsStorage::renameConstraints(const std::string& old_name, const std::string& new_name,
141  const std::string& robot, const std::string& group)
142 {
143  Query::Ptr q = constraints_collection_->createQuery();
144  q->append(CONSTRAINTS_ID_NAME, old_name);
145  if (!robot.empty())
146  q->append(ROBOT_NAME, robot);
147  if (!group.empty())
148  q->append(CONSTRAINTS_GROUP_NAME, group);
149  Metadata::Ptr m = constraints_collection_->createMetadata();
150  m->append(CONSTRAINTS_ID_NAME, new_name);
151  constraints_collection_->modifyMetadata(q, m);
152  ROS_DEBUG("Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
153 }
154 
155 void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string& name, const std::string& robot,
156  const std::string& group)
157 {
158  Query::Ptr q = constraints_collection_->createQuery();
159  q->append(CONSTRAINTS_ID_NAME, name);
160  if (!robot.empty())
161  q->append(ROBOT_NAME, robot);
162  if (!group.empty())
163  q->append(CONSTRAINTS_GROUP_NAME, group);
164  unsigned int rem = constraints_collection_->removeMessages(q);
165  ROS_DEBUG("Removed %u Constraints messages (named '%s')", rem, name.c_str());
166 }
static const std::string DATABASE_NAME
static const std::string CONSTRAINTS_GROUP_NAME
void addConstraints(const moveit_msgs::Constraints &msg, const std::string &robot="", const std::string &group="")
void filterNames(const std::string &regex, std::vector< std::string > &names) const
Keep only the names that match regex.
void removeConstraints(const std::string &name, const std::string &robot="", const std::string &group="")
warehouse_ros::DatabaseConnection::Ptr conn_
This class provides the mechanism to connect to a database and reads needed ROS parameters when appro...
bool hasConstraints(const std::string &name, const std::string &robot="", const std::string &group="") const
void renameConstraints(const std::string &old_name, const std::string &new_name, const std::string &robot="", const std::string &group="")
void getKnownConstraints(std::vector< std::string > &names, const std::string &robot="", const std::string &group="") const
bool getConstraints(ConstraintsWithMetadata &msg_m, const std::string &name, const std::string &robot="", const std::string &group="") const
Get the constraints named name. Return false on failure.
static const std::string CONSTRAINTS_ID_NAME
ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn)
ConstraintsCollection constraints_collection_
#define ROS_DEBUG(...)


warehouse
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:37