planning_scene_world_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::PlanningSceneWorldStorage::DATABASE_NAME = "moveit_planning_scene_worlds";
43 
46 
48  : MoveItMessageStorage(std::move(conn))
49 {
51 }
52 
54 {
56  conn_->openCollectionPtr<moveit_msgs::PlanningSceneWorld>(DATABASE_NAME, "planning_scene_worlds");
57 }
58 
60 {
62  conn_->dropDatabase(DATABASE_NAME);
64 }
65 
66 void moveit_warehouse::PlanningSceneWorldStorage::addPlanningSceneWorld(const moveit_msgs::PlanningSceneWorld& msg,
67  const std::string& name)
68 {
69  bool replace = false;
70  if (hasPlanningSceneWorld(name))
71  {
73  replace = true;
74  }
75  Metadata::Ptr metadata = planning_scene_world_collection_->createMetadata();
76  metadata->append(PLANNING_SCENE_WORLD_ID_NAME, name);
77  planning_scene_world_collection_->insert(msg, metadata);
78  ROS_DEBUG("%s planning scene world '%s'", replace ? "Replaced" : "Added", name.c_str());
79 }
80 
82 {
83  Query::Ptr q = planning_scene_world_collection_->createQuery();
84  q->append(PLANNING_SCENE_WORLD_ID_NAME, name);
85  std::vector<PlanningSceneWorldWithMetadata> psw = planning_scene_world_collection_->queryList(q, true);
86  return !psw.empty();
87 }
88 
90  std::vector<std::string>& names) const
91 {
93  filterNames(regex, names);
94 }
95 
97 {
98  names.clear();
99  Query::Ptr q = planning_scene_world_collection_->createQuery();
100  std::vector<PlanningSceneWorldWithMetadata> constr =
102  for (std::size_t i = 0; i < constr.size(); ++i)
103  if (constr[i]->lookupField(PLANNING_SCENE_WORLD_ID_NAME))
104  names.push_back(constr[i]->lookupString(PLANNING_SCENE_WORLD_ID_NAME));
105 }
106 
108  const std::string& name) const
109 {
110  Query::Ptr q = planning_scene_world_collection_->createQuery();
111  q->append(PLANNING_SCENE_WORLD_ID_NAME, name);
112  std::vector<PlanningSceneWorldWithMetadata> psw = planning_scene_world_collection_->queryList(q, false);
113  if (psw.empty())
114  return false;
115  else
116  {
117  msg_m = psw.front();
118  return true;
119  }
120 }
121 
123  const std::string& new_name)
124 {
125  Query::Ptr q = planning_scene_world_collection_->createQuery();
126  q->append(PLANNING_SCENE_WORLD_ID_NAME, old_name);
127  Metadata::Ptr m = planning_scene_world_collection_->createMetadata();
128  m->append(PLANNING_SCENE_WORLD_ID_NAME, new_name);
129  planning_scene_world_collection_->modifyMetadata(q, m);
130  ROS_DEBUG("Renamed planning scene world from '%s' to '%s'", old_name.c_str(), new_name.c_str());
131 }
132 
134 {
135  Query::Ptr q = planning_scene_world_collection_->createQuery();
136  q->append(PLANNING_SCENE_WORLD_ID_NAME, name);
137  unsigned int rem = planning_scene_world_collection_->removeMessages(q);
138  ROS_DEBUG("Removed %u PlanningSceneWorld messages (named '%s')", rem, name.c_str());
139 }
bool hasPlanningSceneWorld(const std::string &name) const
void getKnownPlanningSceneWorlds(std::vector< std::string > &names) const
void addPlanningSceneWorld(const moveit_msgs::PlanningSceneWorld &msg, const std::string &name)
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 renamePlanningSceneWorld(const std::string &old_name, const std::string &new_name)
bool getPlanningSceneWorld(PlanningSceneWorldWithMetadata &msg_m, const std::string &name) const
Get the constraints named name. Return false on failure.
PlanningSceneWorldStorage(warehouse_ros::DatabaseConnection::Ptr conn)
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 Wed Jun 23 2021 02:53:20