trajectory_constraints_storage.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, 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: Mario Prats, Ioan Sucan */
36 
38 
39 #include <utility>
40 
41 const std::string moveit_warehouse::TrajectoryConstraintsStorage::DATABASE_NAME = "moveit_trajectory_constraints";
42 
46 
49 
51  : MoveItMessageStorage(std::move(conn))
52 {
54 }
55 
57 {
59  conn_->openCollectionPtr<moveit_msgs::TrajectoryConstraints>(DATABASE_NAME, "trajectory_constraints");
60 }
61 
63 {
65  conn_->dropDatabase(DATABASE_NAME);
67 }
68 
70  const moveit_msgs::TrajectoryConstraints& msg, const std::string& name, const std::string& robot,
71  const std::string& group)
72 {
73  bool replace = false;
74  if (hasTrajectoryConstraints(name, robot, group))
75  {
76  removeTrajectoryConstraints(name, robot, group);
77  replace = true;
78  }
79  Metadata::Ptr metadata = constraints_collection_->createMetadata();
80  metadata->append(CONSTRAINTS_ID_NAME, name);
81  metadata->append(ROBOT_NAME, robot);
82  metadata->append(CONSTRAINTS_GROUP_NAME, group);
83  constraints_collection_->insert(msg, metadata);
84  ROS_DEBUG("%s constraints '%s'", replace ? "Replaced" : "Added", name.c_str());
85 }
86 
88  const std::string& robot,
89  const std::string& group) const
90 {
91  Query::Ptr q = constraints_collection_->createQuery();
92  q->append(CONSTRAINTS_ID_NAME, name);
93  if (!robot.empty())
94  q->append(ROBOT_NAME, robot);
95  if (!group.empty())
96  q->append(CONSTRAINTS_GROUP_NAME, group);
97  std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->queryList(q, true);
98  return !constr.empty();
99 }
100 
102  std::vector<std::string>& names,
103  const std::string& robot,
104  const std::string& group) const
105 {
106  getKnownTrajectoryConstraints(names, robot, group);
107  filterNames(regex, names);
108 }
109 
111  const std::string& robot,
112  const std::string& group) const
113 {
114  names.clear();
115  Query::Ptr q = constraints_collection_->createQuery();
116  if (!robot.empty())
117  q->append(ROBOT_NAME, robot);
118  if (!group.empty())
119  q->append(CONSTRAINTS_GROUP_NAME, group);
120  std::vector<TrajectoryConstraintsWithMetadata> constr =
121  constraints_collection_->queryList(q, true, CONSTRAINTS_ID_NAME, true);
122  for (std::size_t i = 0; i < constr.size(); ++i)
123  if (constr[i]->lookupField(CONSTRAINTS_ID_NAME))
124  names.push_back(constr[i]->lookupString(CONSTRAINTS_ID_NAME));
125 }
126 
128  const std::string& name,
129  const std::string& robot,
130  const std::string& group) const
131 {
132  Query::Ptr q = constraints_collection_->createQuery();
133  q->append(CONSTRAINTS_ID_NAME, name);
134  if (!robot.empty())
135  q->append(ROBOT_NAME, robot);
136  if (!group.empty())
137  q->append(CONSTRAINTS_GROUP_NAME, group);
138  std::vector<TrajectoryConstraintsWithMetadata> constr = constraints_collection_->queryList(q, false);
139  if (constr.empty())
140  return false;
141  else
142  {
143  msg_m = constr.back();
144  return true;
145  }
146 }
147 
149  const std::string& new_name,
150  const std::string& robot,
151  const std::string& group)
152 {
153  Query::Ptr q = constraints_collection_->createQuery();
154  q->append(CONSTRAINTS_ID_NAME, old_name);
155  if (!robot.empty())
156  q->append(ROBOT_NAME, robot);
157  if (!group.empty())
158  q->append(CONSTRAINTS_GROUP_NAME, group);
159  Metadata::Ptr m = constraints_collection_->createMetadata();
160  m->append(CONSTRAINTS_ID_NAME, new_name);
161  constraints_collection_->modifyMetadata(q, m);
162  ROS_DEBUG("Renamed constraints from '%s' to '%s'", old_name.c_str(), new_name.c_str());
163 }
164 
166  const std::string& robot,
167  const std::string& group)
168 {
169  Query::Ptr q = constraints_collection_->createQuery();
170  q->append(CONSTRAINTS_ID_NAME, name);
171  if (!robot.empty())
172  q->append(ROBOT_NAME, robot);
173  if (!group.empty())
174  q->append(CONSTRAINTS_GROUP_NAME, group);
175  unsigned int rem = constraints_collection_->removeMessages(q);
176  ROS_DEBUG("Removed %u TrajectoryConstraints messages (named '%s')", rem, name.c_str());
177 }
bool getTrajectoryConstraints(TrajectoryConstraintsWithMetadata &msg_m, const std::string &name, const std::string &robot="", const std::string &group="") const
Get the constraints named name. Return false on failure.
bool hasTrajectoryConstraints(const std::string &name, const std::string &robot="", const std::string &group="") const
warehouse_ros::DatabaseConnection::Ptr conn_
void addTrajectoryConstraints(const moveit_msgs::TrajectoryConstraints &msg, const std::string &name, const std::string &robot="", const std::string &group="")
This class provides the mechanism to connect to a database and reads needed ROS parameters when appro...
#define ROS_DEBUG(...)
TrajectoryConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn)
void removeTrajectoryConstraints(const std::string &name, const std::string &robot="", const std::string &group="")
void renameTrajectoryConstraints(const std::string &old_name, const std::string &new_name, 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 getKnownTrajectoryConstraints(std::vector< std::string > &names, const std::string &robot="", const std::string &group="") const


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