planning_scene_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 #include <boost/regex.hpp>
39 #include <utility>
40 
41 const std::string moveit_warehouse::PlanningSceneStorage::DATABASE_NAME = "moveit_planning_scenes";
42 
43 const std::string moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME = "planning_scene_id";
44 const std::string moveit_warehouse::PlanningSceneStorage::MOTION_PLAN_REQUEST_ID_NAME = "motion_request_id";
45 
48 
50  : MoveItMessageStorage(std::move(conn))
51 {
53 }
54 
56 {
57  planning_scene_collection_ = conn_->openCollectionPtr<moveit_msgs::PlanningScene>(DATABASE_NAME, "planning_scene");
59  conn_->openCollectionPtr<moveit_msgs::MotionPlanRequest>(DATABASE_NAME, "motion_plan_request");
61  conn_->openCollectionPtr<moveit_msgs::RobotTrajectory>(DATABASE_NAME, "robot_trajectory");
62 }
63 
65 {
69  conn_->dropDatabase(DATABASE_NAME);
71 }
72 
73 void moveit_warehouse::PlanningSceneStorage::addPlanningScene(const moveit_msgs::PlanningScene& scene)
74 {
75  bool replace = false;
76  if (hasPlanningScene(scene.name))
77  {
78  removePlanningScene(scene.name);
79  replace = true;
80  }
81  Metadata::Ptr metadata = planning_scene_collection_->createMetadata();
82  metadata->append(PLANNING_SCENE_ID_NAME, scene.name);
83  planning_scene_collection_->insert(scene, metadata);
84  ROS_DEBUG("%s scene '%s'", replace ? "Replaced" : "Added", scene.name.c_str());
85 }
86 
88 {
89  Query::Ptr q = planning_scene_collection_->createQuery();
90  q->append(PLANNING_SCENE_ID_NAME, name);
91  std::vector<PlanningSceneWithMetadata> planning_scenes = planning_scene_collection_->queryList(q, true);
92  return !planning_scenes.empty();
93 }
94 
95 std::string
96 moveit_warehouse::PlanningSceneStorage::getMotionPlanRequestName(const moveit_msgs::MotionPlanRequest& planning_query,
97  const std::string& scene_name) const
98 {
99  // get all existing motion planning requests for this planning scene
100  Query::Ptr q = motion_plan_request_collection_->createQuery();
101  q->append(PLANNING_SCENE_ID_NAME, scene_name);
102  std::vector<MotionPlanRequestWithMetadata> existing_requests = motion_plan_request_collection_->queryList(q, false);
103 
104  // if there are no requests stored, we are done
105  if (existing_requests.empty())
106  return "";
107 
108  // compute the serialization of the message passed as argument
109  const size_t serial_size_arg = ros::serialization::serializationLength(planning_query);
110  boost::shared_array<uint8_t> buffer_arg(new uint8_t[serial_size_arg]);
111  ros::serialization::OStream stream_arg(buffer_arg.get(), serial_size_arg);
112  ros::serialization::serialize(stream_arg, planning_query);
113  const void* data_arg = buffer_arg.get();
114 
115  for (std::size_t i = 0; i < existing_requests.size(); ++i)
116  {
117  const size_t serial_size = ros::serialization::serializationLength(
118  static_cast<const moveit_msgs::MotionPlanRequest&>(*existing_requests[i]));
119  if (serial_size != serial_size_arg)
120  continue;
121  boost::shared_array<uint8_t> buffer(new uint8_t[serial_size]);
122  ros::serialization::OStream stream(buffer.get(), serial_size);
123  ros::serialization::serialize(stream, static_cast<const moveit_msgs::MotionPlanRequest&>(*existing_requests[i]));
124  const void* data = buffer.get();
125  if (memcmp(data_arg, data, serial_size) == 0)
126  // we found the same message twice
127  return existing_requests[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME);
128  }
129  return "";
130 }
131 
132 void moveit_warehouse::PlanningSceneStorage::addPlanningQuery(const moveit_msgs::MotionPlanRequest& planning_query,
133  const std::string& scene_name,
134  const std::string& query_name)
135 {
136  std::string id = getMotionPlanRequestName(planning_query, scene_name);
137 
138  // if we are trying to overwrite, we remove the old query first (if it exists).
139  if (!query_name.empty() && id.empty())
140  removePlanningQuery(scene_name, query_name);
141 
142  if (id != query_name || id.empty())
143  addNewPlanningRequest(planning_query, scene_name, query_name);
144 }
145 
147  const moveit_msgs::MotionPlanRequest& planning_query, const std::string& scene_name, const std::string& query_name)
148 {
149  std::string id = query_name;
150  if (id.empty())
151  {
152  std::set<std::string> used;
153  Query::Ptr q = motion_plan_request_collection_->createQuery();
154  q->append(PLANNING_SCENE_ID_NAME, scene_name);
155  std::vector<MotionPlanRequestWithMetadata> existing_requests = motion_plan_request_collection_->queryList(q, true);
156  for (std::size_t i = 0; i < existing_requests.size(); ++i)
157  used.insert(existing_requests[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME));
158  std::size_t index = existing_requests.size();
159  do
160  {
161  id = "Motion Plan Request " + boost::lexical_cast<std::string>(index);
162  index++;
163  } while (used.find(id) != used.end());
164  }
165  Metadata::Ptr metadata = motion_plan_request_collection_->createMetadata();
166  metadata->append(PLANNING_SCENE_ID_NAME, scene_name);
167  metadata->append(MOTION_PLAN_REQUEST_ID_NAME, id);
168  motion_plan_request_collection_->insert(planning_query, metadata);
169  ROS_DEBUG("Saved planning query '%s' for scene '%s'", id.c_str(), scene_name.c_str());
170  return id;
171 }
172 
173 void moveit_warehouse::PlanningSceneStorage::addPlanningResult(const moveit_msgs::MotionPlanRequest& planning_query,
174  const moveit_msgs::RobotTrajectory& result,
175  const std::string& scene_name)
176 {
177  std::string id = getMotionPlanRequestName(planning_query, scene_name);
178  if (id.empty())
179  id = addNewPlanningRequest(planning_query, scene_name, "");
180  Metadata::Ptr metadata = robot_trajectory_collection_->createMetadata();
181  metadata->append(PLANNING_SCENE_ID_NAME, scene_name);
182  metadata->append(MOTION_PLAN_REQUEST_ID_NAME, id);
183  robot_trajectory_collection_->insert(result, metadata);
184 }
185 
186 void moveit_warehouse::PlanningSceneStorage::getPlanningSceneNames(std::vector<std::string>& names) const
187 {
188  names.clear();
189  Query::Ptr q = planning_scene_collection_->createQuery();
190  std::vector<PlanningSceneWithMetadata> planning_scenes =
191  planning_scene_collection_->queryList(q, true, PLANNING_SCENE_ID_NAME, true);
192  for (std::size_t i = 0; i < planning_scenes.size(); ++i)
193  if (planning_scenes[i]->lookupField(PLANNING_SCENE_ID_NAME))
194  names.push_back(planning_scenes[i]->lookupString(PLANNING_SCENE_ID_NAME));
195 }
196 
198  std::vector<std::string>& names) const
199 {
200  getPlanningSceneNames(names);
201  filterNames(regex, names);
202 }
203 
204 bool moveit_warehouse::PlanningSceneStorage::getPlanningSceneWorld(moveit_msgs::PlanningSceneWorld& world,
205  const std::string& scene_name) const
206 {
208  if (getPlanningScene(scene_m, scene_name))
209  {
210  world = scene_m->world;
211  return true;
212  }
213  else
214  return false;
215 }
216 
218  const std::string& scene_name) const
219 {
220  Query::Ptr q = planning_scene_collection_->createQuery();
221  q->append(PLANNING_SCENE_ID_NAME, scene_name);
222  std::vector<PlanningSceneWithMetadata> planning_scenes = planning_scene_collection_->queryList(q, false);
223  if (planning_scenes.empty())
224  {
225  ROS_WARN("Planning scene '%s' was not found in the database", scene_name.c_str());
226  return false;
227  }
228  scene_m = planning_scenes.back();
229  // in case the scene was renamed, the name in the message may be out of date
230  const_cast<moveit_msgs::PlanningScene*>(static_cast<const moveit_msgs::PlanningScene*>(scene_m.get()))->name =
231  scene_name;
232  return true;
233 }
234 
236  const std::string& scene_name,
237  const std::string& query_name)
238 {
239  Query::Ptr q = motion_plan_request_collection_->createQuery();
240  q->append(PLANNING_SCENE_ID_NAME, scene_name);
241  q->append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
242  std::vector<MotionPlanRequestWithMetadata> planning_queries = motion_plan_request_collection_->queryList(q, false);
243  if (planning_queries.empty())
244  {
245  ROS_ERROR("Planning query '%s' not found for scene '%s'", query_name.c_str(), scene_name.c_str());
246  return false;
247  }
248  else
249  {
250  query_m = planning_queries.front();
251  return true;
252  }
253 }
254 
256  std::vector<MotionPlanRequestWithMetadata>& planning_queries, const std::string& scene_name) const
257 {
258  Query::Ptr q = motion_plan_request_collection_->createQuery();
259  q->append(PLANNING_SCENE_ID_NAME, scene_name);
260  planning_queries = motion_plan_request_collection_->queryList(q, false);
261 }
262 
263 void moveit_warehouse::PlanningSceneStorage::getPlanningQueriesNames(std::vector<std::string>& query_names,
264  const std::string& scene_name) const
265 {
266  Query::Ptr q = motion_plan_request_collection_->createQuery();
267  q->append(PLANNING_SCENE_ID_NAME, scene_name);
268  std::vector<MotionPlanRequestWithMetadata> planning_queries = motion_plan_request_collection_->queryList(q, true);
269  query_names.clear();
270  for (std::size_t i = 0; i < planning_queries.size(); ++i)
271  if (planning_queries[i]->lookupField(MOTION_PLAN_REQUEST_ID_NAME))
272  query_names.push_back(planning_queries[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME));
273 }
274 
276  std::vector<std::string>& query_names,
277  const std::string& scene_name) const
278 {
279  getPlanningQueriesNames(query_names, scene_name);
280 
281  if (!regex.empty())
282  {
283  std::vector<std::string> fnames;
284  boost::regex r(regex);
285  for (std::size_t i = 0; i < query_names.size(); ++i)
286  {
287  boost::cmatch match;
288  if (boost::regex_match(query_names[i].c_str(), match, r))
289  {
290  fnames.push_back(query_names[i]);
291  }
292  }
293  query_names.swap(fnames);
294  }
295 }
296 
298  std::vector<MotionPlanRequestWithMetadata>& planning_queries, std::vector<std::string>& query_names,
299  const std::string& scene_name) const
300 {
301  Query::Ptr q = motion_plan_request_collection_->createQuery();
302  q->append(PLANNING_SCENE_ID_NAME, scene_name);
303  planning_queries = motion_plan_request_collection_->queryList(q, false);
304  query_names.resize(planning_queries.size());
305  for (std::size_t i = 0; i < planning_queries.size(); ++i)
306  if (planning_queries[i]->lookupField(MOTION_PLAN_REQUEST_ID_NAME))
307  query_names[i] = planning_queries[i]->lookupString(MOTION_PLAN_REQUEST_ID_NAME);
308  else
309  query_names[i].clear();
310 }
311 
313  std::vector<RobotTrajectoryWithMetadata>& planning_results, const std::string& scene_name,
314  const moveit_msgs::MotionPlanRequest& planning_query) const
315 {
316  std::string id = getMotionPlanRequestName(planning_query, scene_name);
317  if (id.empty())
318  planning_results.clear();
319  else
320  getPlanningResults(planning_results, id, scene_name);
321 }
322 
324  std::vector<RobotTrajectoryWithMetadata>& planning_results, const std::string& scene_name,
325  const std::string& planning_query) const
326 {
327  Query::Ptr q = robot_trajectory_collection_->createQuery();
328  q->append(PLANNING_SCENE_ID_NAME, scene_name);
329  q->append(MOTION_PLAN_REQUEST_ID_NAME, planning_query);
330  planning_results = robot_trajectory_collection_->queryList(q, false);
331 }
332 
334  const std::string& query_name) const
335 {
336  Query::Ptr q = motion_plan_request_collection_->createQuery();
337  q->append(PLANNING_SCENE_ID_NAME, scene_name);
338  q->append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
339  std::vector<MotionPlanRequestWithMetadata> queries = motion_plan_request_collection_->queryList(q, true);
340  return !queries.empty();
341 }
342 
343 void moveit_warehouse::PlanningSceneStorage::renamePlanningScene(const std::string& old_scene_name,
344  const std::string& new_scene_name)
345 {
346  Query::Ptr q = planning_scene_collection_->createQuery();
347  q->append(PLANNING_SCENE_ID_NAME, old_scene_name);
348  Metadata::Ptr m = planning_scene_collection_->createMetadata();
349  m->append(PLANNING_SCENE_ID_NAME, new_scene_name);
350  planning_scene_collection_->modifyMetadata(q, m);
351  ROS_DEBUG("Renamed planning scene from '%s' to '%s'", old_scene_name.c_str(), new_scene_name.c_str());
352 }
353 
355  const std::string& old_query_name,
356  const std::string& new_query_name)
357 {
358  Query::Ptr q = motion_plan_request_collection_->createQuery();
359  q->append(PLANNING_SCENE_ID_NAME, scene_name);
360  q->append(MOTION_PLAN_REQUEST_ID_NAME, old_query_name);
361  Metadata::Ptr m = motion_plan_request_collection_->createMetadata();
362  m->append(MOTION_PLAN_REQUEST_ID_NAME, new_query_name);
363  motion_plan_request_collection_->modifyMetadata(q, m);
364  ROS_DEBUG("Renamed planning query for scene '%s' from '%s' to '%s'", scene_name.c_str(), old_query_name.c_str(),
365  new_query_name.c_str());
366 }
367 
369 {
370  removePlanningQueries(scene_name);
371  Query::Ptr q = planning_scene_collection_->createQuery();
372  q->append(PLANNING_SCENE_ID_NAME, scene_name);
373  unsigned int rem = planning_scene_collection_->removeMessages(q);
374  ROS_DEBUG("Removed %u PlanningScene messages (named '%s')", rem, scene_name.c_str());
375 }
376 
378 {
379  removePlanningResults(scene_name);
380  Query::Ptr q = motion_plan_request_collection_->createQuery();
381  q->append(PLANNING_SCENE_ID_NAME, scene_name);
382  unsigned int rem = motion_plan_request_collection_->removeMessages(q);
383  ROS_DEBUG("Removed %u MotionPlanRequest messages for scene '%s'", rem, scene_name.c_str());
384 }
385 
387  const std::string& query_name)
388 {
389  removePlanningResults(scene_name, query_name);
390  Query::Ptr q = motion_plan_request_collection_->createQuery();
391  q->append(PLANNING_SCENE_ID_NAME, scene_name);
392  q->append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
393  unsigned int rem = motion_plan_request_collection_->removeMessages(q);
394  ROS_DEBUG("Removed %u MotionPlanRequest messages for scene '%s', query '%s'", rem, scene_name.c_str(),
395  query_name.c_str());
396 }
397 
399 {
400  Query::Ptr q = robot_trajectory_collection_->createQuery();
401  q->append(PLANNING_SCENE_ID_NAME, scene_name);
402  unsigned int rem = robot_trajectory_collection_->removeMessages(q);
403  ROS_DEBUG("Removed %u RobotTrajectory messages for scene '%s'", rem, scene_name.c_str());
404 }
405 
407  const std::string& query_name)
408 {
409  Query::Ptr q = robot_trajectory_collection_->createQuery();
410  q->append(PLANNING_SCENE_ID_NAME, scene_name);
411  q->append(MOTION_PLAN_REQUEST_ID_NAME, query_name);
412  unsigned int rem = robot_trajectory_collection_->removeMessages(q);
413  ROS_DEBUG("Removed %u RobotTrajectory messages for scene '%s', query '%s'", rem, scene_name.c_str(),
414  query_name.c_str());
415 }
void removePlanningQuery(const std::string &scene_name, const std::string &query_name)
void renamePlanningQuery(const std::string &scene_name, const std::string &old_query_name, const std::string &new_query_name)
void getPlanningResults(std::vector< RobotTrajectoryWithMetadata > &planning_results, const std::string &scene_name, const moveit_msgs::MotionPlanRequest &planning_query) const
void removePlanningQueries(const std::string &scene_name)
#define ROS_ERROR(...)
#define ROS_WARN(...)
PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn)
warehouse_ros::DatabaseConnection::Ptr conn_
void renamePlanningScene(const std::string &old_scene_name, const std::string &new_scene_name)
This class provides the mechanism to connect to a database and reads needed ROS parameters when appro...
#define ROS_DEBUG(...)
void getPlanningQueriesNames(std::vector< std::string > &query_names, const std::string &scene_name) const
RobotTrajectoryCollection robot_trajectory_collection_
void getPlanningSceneNames(std::vector< std::string > &names) const
unsigned int index
MotionPlanRequestCollection motion_plan_request_collection_
void serialize(Stream &stream, const T &t)
static const std::string MOTION_PLAN_REQUEST_ID_NAME
bool hasPlanningQuery(const std::string &scene_name, const std::string &query_name) const
void removePlanningScene(const std::string &scene_name)
void addPlanningScene(const moveit_msgs::PlanningScene &scene)
std::string getMotionPlanRequestName(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name) const
bool getPlanningQuery(MotionPlanRequestWithMetadata &query_m, const std::string &scene_name, const std::string &query_name)
bool getPlanningScene(PlanningSceneWithMetadata &scene_m, const std::string &scene_name) const
Get the latest planning scene named scene_name.
uint32_t serializationLength(const T &t)
bool getPlanningSceneWorld(moveit_msgs::PlanningSceneWorld &world, const std::string &scene_name) const
std::string addNewPlanningRequest(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name)
void addPlanningResult(const moveit_msgs::MotionPlanRequest &planning_query, const moveit_msgs::RobotTrajectory &result, const std::string &scene_name)
bool hasPlanningScene(const std::string &name) const
void getPlanningQueries(std::vector< MotionPlanRequestWithMetadata > &planning_queries, const std::string &scene_name) const
r
void removePlanningResults(const std::string &scene_name)
void filterNames(const std::string &regex, std::vector< std::string > &names) const
Keep only the names that match regex.
void addPlanningQuery(const moveit_msgs::MotionPlanRequest &planning_query, const std::string &scene_name, const std::string &query_name="")


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