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 #include <utility>
40 
41 const std::string moveit_warehouse::ConstraintsStorage::DATABASE_NAME = "moveit_constraints";
42 
43 const std::string moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME = "constraints_id";
45 const std::string moveit_warehouse::ConstraintsStorage::ROBOT_NAME = "robot_id";
46 
49 
51  : MoveItMessageStorage(std::move(conn))
52 {
54 }
55 
57 {
58  constraints_collection_ = conn_->openCollectionPtr<moveit_msgs::Constraints>(DATABASE_NAME, "constraints");
59 }
60 
62 {
64  conn_->dropDatabase(DATABASE_NAME);
66 }
67 
68 void moveit_warehouse::ConstraintsStorage::addConstraints(const moveit_msgs::Constraints& msg, const std::string& robot,
69  const std::string& group)
70 {
71  bool replace = false;
72  if (hasConstraints(msg.name, robot, group))
73  {
74  removeConstraints(msg.name, robot, group);
75  replace = true;
76  }
77  Metadata::Ptr metadata = constraints_collection_->createMetadata();
78  metadata->append(CONSTRAINTS_ID_NAME, msg.name);
79  metadata->append(ROBOT_NAME, robot);
80  metadata->append(CONSTRAINTS_GROUP_NAME, group);
81  constraints_collection_->insert(msg, metadata);
82  ROS_DEBUG("%s constraints '%s'", replace ? "Replaced" : "Added", msg.name.c_str());
83 }
84 
85 bool moveit_warehouse::ConstraintsStorage::hasConstraints(const std::string& name, const std::string& robot,
86  const std::string& group) const
87 {
88  Query::Ptr q = constraints_collection_->createQuery();
89  q->append(CONSTRAINTS_ID_NAME, name);
90  if (!robot.empty())
91  q->append(ROBOT_NAME, robot);
92  if (!group.empty())
93  q->append(CONSTRAINTS_GROUP_NAME, group);
94  std::vector<ConstraintsWithMetadata> constr = constraints_collection_->queryList(q, true);
95  return !constr.empty();
96 }
97 
99  std::vector<std::string>& names,
100  const std::string& robot, const std::string& group) const
101 {
102  getKnownConstraints(names, robot, group);
103  filterNames(regex, names);
104 }
105 
107  const std::string& robot, const std::string& group) const
108 {
109  names.clear();
110  Query::Ptr q = constraints_collection_->createQuery();
111  if (!robot.empty())
112  q->append(ROBOT_NAME, robot);
113  if (!group.empty())
114  q->append(CONSTRAINTS_GROUP_NAME, group);
115  std::vector<ConstraintsWithMetadata> constr = constraints_collection_->queryList(q, true, CONSTRAINTS_ID_NAME, true);
116  for (std::size_t i = 0; i < constr.size(); ++i)
117  if (constr[i]->lookupField(CONSTRAINTS_ID_NAME))
118  names.push_back(constr[i]->lookupString(CONSTRAINTS_ID_NAME));
119 }
120 
122  const std::string& robot, const std::string& group) const
123 {
124  Query::Ptr q = constraints_collection_->createQuery();
125  q->append(CONSTRAINTS_ID_NAME, name);
126  if (!robot.empty())
127  q->append(ROBOT_NAME, robot);
128  if (!group.empty())
129  q->append(CONSTRAINTS_GROUP_NAME, group);
130  std::vector<ConstraintsWithMetadata> constr = constraints_collection_->queryList(q, false);
131  if (constr.empty())
132  return false;
133  else
134  {
135  msg_m = constr.back();
136  // in case the constraints were renamed, the name in the message may be out of date
137  const_cast<moveit_msgs::Constraints*>(static_cast<const moveit_msgs::Constraints*>(msg_m.get()))->name = name;
138  return true;
139  }
140 }
141 
142 void moveit_warehouse::ConstraintsStorage::renameConstraints(const std::string& old_name, const std::string& new_name,
143  const std::string& robot, const std::string& group)
144 {
145  Query::Ptr q = constraints_collection_->createQuery();
146  q->append(CONSTRAINTS_ID_NAME, old_name);
147  if (!robot.empty())
148  q->append(ROBOT_NAME, robot);
149  if (!group.empty())
150  q->append(CONSTRAINTS_GROUP_NAME, group);
151  Metadata::Ptr m = constraints_collection_->createMetadata();
152  m->append(CONSTRAINTS_ID_NAME, new_name);
153  constraints_collection_->modifyMetadata(q, m);
154  ROS_DEBUG("Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
155 }
156 
157 void moveit_warehouse::ConstraintsStorage::removeConstraints(const std::string& name, const std::string& robot,
158  const std::string& group)
159 {
160  Query::Ptr q = constraints_collection_->createQuery();
161  q->append(CONSTRAINTS_ID_NAME, name);
162  if (!robot.empty())
163  q->append(ROBOT_NAME, robot);
164  if (!group.empty())
165  q->append(CONSTRAINTS_GROUP_NAME, group);
166  unsigned int rem = constraints_collection_->removeMessages(q);
167  ROS_DEBUG("Removed %u Constraints messages (named '%s')", rem, name.c_str());
168 }
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 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...
#define ROS_DEBUG(...)
void renameConstraints(const std::string &old_name, const std::string &new_name, const std::string &robot="", const std::string &group="")
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
void getKnownConstraints(std::vector< std::string > &names, const std::string &robot="", const std::string &group="") const
ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn)
bool hasConstraints(const std::string &name, const std::string &robot="", const std::string &group="") const
ConstraintsCollection constraints_collection_
void filterNames(const std::string &regex, std::vector< std::string > &names) const
Keep only the names that match regex.


warehouse
Author(s): Ioan Sucan
autogenerated on Fri Aug 20 2021 03:01:32