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


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