planning_scene_interface.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, Sachin Chitta */
36 
39 #include <moveit_msgs/GetPlanningScene.h>
40 #include <moveit_msgs/ApplyPlanningScene.h>
41 #include <ros/ros.h>
42 #include <algorithm>
43 
44 namespace moveit
45 {
46 namespace planning_interface
47 {
48 static const std::string LOGNAME = "planning_scene_interface";
49 
51 {
52 public:
53  explicit PlanningSceneInterfaceImpl(const std::string& ns = "", bool wait = true)
54  {
56  planning_scene_diff_publisher_ = node_handle_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
61 
62  if (wait)
63  {
64  waitForService(planning_scene_service_);
65  waitForService(apply_planning_scene_service_);
66  }
67  else
68  {
69  if (!planning_scene_service_.exists() || !apply_planning_scene_service_.exists())
70  {
71  throw std::runtime_error("ROS services not available");
72  }
73  }
74  }
75 
76  std::vector<std::string> getKnownObjectNames(bool with_type)
77  {
78  moveit_msgs::GetPlanningScene::Request request;
79  moveit_msgs::GetPlanningScene::Response response;
80  std::vector<std::string> result;
81  request.components.components = request.components.WORLD_OBJECT_NAMES;
82  if (!planning_scene_service_.call(request, response))
83  return result;
84  if (with_type)
85  {
86  for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
87  if (!response.scene.world.collision_objects[i].type.key.empty())
88  result.push_back(response.scene.world.collision_objects[i].id);
89  }
90  else
91  {
92  for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
93  result.push_back(response.scene.world.collision_objects[i].id);
94  }
95  return result;
96  }
97 
98  std::vector<std::string> getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy,
99  double maxz, bool with_type, std::vector<std::string>& types)
100  {
101  moveit_msgs::GetPlanningScene::Request request;
102  moveit_msgs::GetPlanningScene::Response response;
103  std::vector<std::string> result;
104  request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
105  if (!planning_scene_service_.call(request, response))
106  {
107  ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object names");
108  return result;
109  }
110 
111  for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
112  {
113  if (with_type && response.scene.world.collision_objects[i].type.key.empty())
114  continue;
115  if (response.scene.world.collision_objects[i].mesh_poses.empty() &&
116  response.scene.world.collision_objects[i].primitive_poses.empty())
117  continue;
118  bool good = true;
119  for (std::size_t j = 0; j < response.scene.world.collision_objects[i].mesh_poses.size(); ++j)
120  if (!(response.scene.world.collision_objects[i].mesh_poses[j].position.x >= minx &&
121  response.scene.world.collision_objects[i].mesh_poses[j].position.x <= maxx &&
122  response.scene.world.collision_objects[i].mesh_poses[j].position.y >= miny &&
123  response.scene.world.collision_objects[i].mesh_poses[j].position.y <= maxy &&
124  response.scene.world.collision_objects[i].mesh_poses[j].position.z >= minz &&
125  response.scene.world.collision_objects[i].mesh_poses[j].position.z <= maxz))
126  {
127  good = false;
128  break;
129  }
130  for (std::size_t j = 0; j < response.scene.world.collision_objects[i].primitive_poses.size(); ++j)
131  if (!(response.scene.world.collision_objects[i].primitive_poses[j].position.x >= minx &&
132  response.scene.world.collision_objects[i].primitive_poses[j].position.x <= maxx &&
133  response.scene.world.collision_objects[i].primitive_poses[j].position.y >= miny &&
134  response.scene.world.collision_objects[i].primitive_poses[j].position.y <= maxy &&
135  response.scene.world.collision_objects[i].primitive_poses[j].position.z >= minz &&
136  response.scene.world.collision_objects[i].primitive_poses[j].position.z <= maxz))
137  {
138  good = false;
139  break;
140  }
141  if (good)
142  {
143  result.push_back(response.scene.world.collision_objects[i].id);
144  if (with_type)
145  types.push_back(response.scene.world.collision_objects[i].type.key);
146  }
147  }
148  return result;
149  }
150 
151  std::map<std::string, geometry_msgs::Pose> getObjectPoses(const std::vector<std::string>& object_ids)
152  {
153  moveit_msgs::GetPlanningScene::Request request;
154  moveit_msgs::GetPlanningScene::Response response;
155  std::map<std::string, geometry_msgs::Pose> result;
156  request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
157  if (!planning_scene_service_.call(request, response))
158  {
159  ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object names");
160  return result;
161  }
162 
163  for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
164  {
165  if (std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) !=
166  object_ids.end())
167  {
168  if (response.scene.world.collision_objects[i].mesh_poses.empty() &&
169  response.scene.world.collision_objects[i].primitive_poses.empty())
170  continue;
171  if (!response.scene.world.collision_objects[i].mesh_poses.empty())
172  result[response.scene.world.collision_objects[i].id] =
173  response.scene.world.collision_objects[i].mesh_poses[0];
174  else
175  result[response.scene.world.collision_objects[i].id] =
176  response.scene.world.collision_objects[i].primitive_poses[0];
177  }
178  }
179  return result;
180  }
181 
182  std::map<std::string, moveit_msgs::CollisionObject> getObjects(const std::vector<std::string>& object_ids)
183  {
184  moveit_msgs::GetPlanningScene::Request request;
185  moveit_msgs::GetPlanningScene::Response response;
186  std::map<std::string, moveit_msgs::CollisionObject> result;
187  request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
188  if (!planning_scene_service_.call(request, response))
189  {
190  ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object geometries");
191  return result;
192  }
193 
194  for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
195  {
196  if (object_ids.empty() ||
197  std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) !=
198  object_ids.end())
199  {
200  result[response.scene.world.collision_objects[i].id] = response.scene.world.collision_objects[i];
201  }
202  }
203  return result;
204  }
205 
206  std::map<std::string, moveit_msgs::AttachedCollisionObject>
207  getAttachedObjects(const std::vector<std::string>& object_ids)
208  {
209  moveit_msgs::GetPlanningScene::Request request;
210  moveit_msgs::GetPlanningScene::Response response;
211  std::map<std::string, moveit_msgs::AttachedCollisionObject> result;
212  request.components.components = request.components.ROBOT_STATE_ATTACHED_OBJECTS;
213  if (!planning_scene_service_.call(request, response))
214  {
215  ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get attached object geometries");
216  return result;
217  }
218 
219  for (std::size_t i = 0; i < response.scene.robot_state.attached_collision_objects.size(); ++i)
220  {
221  if (object_ids.empty() ||
222  std::find(object_ids.begin(), object_ids.end(),
223  response.scene.robot_state.attached_collision_objects[i].object.id) != object_ids.end())
224  {
225  result[response.scene.robot_state.attached_collision_objects[i].object.id] =
226  response.scene.robot_state.attached_collision_objects[i];
227  }
228  }
229  return result;
230  }
231 
232  bool applyPlanningScene(const moveit_msgs::PlanningScene& planning_scene)
233  {
234  moveit_msgs::ApplyPlanningScene::Request request;
235  moveit_msgs::ApplyPlanningScene::Response response;
236  request.scene = planning_scene;
237  if (!apply_planning_scene_service_.call(request, response))
238  {
239  ROS_WARN_NAMED(LOGNAME, "Failed to call ApplyPlanningScene service");
240  return false;
241  }
242  return response.success;
243  }
244 
245  void addCollisionObjects(const std::vector<moveit_msgs::CollisionObject>& collision_objects,
246  const std::vector<moveit_msgs::ObjectColor>& object_colors) const
247  {
248  moveit_msgs::PlanningScene planning_scene;
249  planning_scene.world.collision_objects = collision_objects;
250  planning_scene.object_colors = object_colors;
251 
252  for (size_t i = 0; i < planning_scene.object_colors.size(); ++i)
253  {
254  if (planning_scene.object_colors[i].id.empty() && i < collision_objects.size())
255  planning_scene.object_colors[i].id = collision_objects[i].id;
256  else
257  break;
258  }
259 
260  planning_scene.is_diff = true;
261  planning_scene_diff_publisher_.publish(planning_scene);
262  }
263 
264  void removeCollisionObjects(const std::vector<std::string>& object_ids) const
265  {
266  moveit_msgs::PlanningScene planning_scene;
267  moveit_msgs::CollisionObject object;
268  for (std::size_t i = 0; i < object_ids.size(); ++i)
269  {
270  object.id = object_ids[i];
271  object.operation = object.REMOVE;
272  planning_scene.world.collision_objects.push_back(object);
273  }
274  planning_scene.is_diff = true;
275  planning_scene_diff_publisher_.publish(planning_scene);
276  }
277 
278 private:
280  {
281  ros::Duration time_before_warning(5.0);
282  srv.waitForExistence(time_before_warning);
283  if (!srv.exists())
284  {
285  ROS_WARN_STREAM_NAMED(LOGNAME, "service '" << srv.getService() << "' not advertised yet. Continue waiting...");
286  srv.waitForExistence();
287  }
288  }
289 
294  robot_model::RobotModelConstPtr robot_model_;
295 };
296 
297 PlanningSceneInterface::PlanningSceneInterface(const std::string& ns, bool wait)
298 {
299  impl_ = new PlanningSceneInterfaceImpl(ns, wait);
300 }
301 
303 {
304  delete impl_;
305 }
306 
307 std::vector<std::string> PlanningSceneInterface::getKnownObjectNames(bool with_type)
308 {
309  return impl_->getKnownObjectNames(with_type);
310 }
311 
312 std::vector<std::string> PlanningSceneInterface::getKnownObjectNamesInROI(double minx, double miny, double minz,
313  double maxx, double maxy, double maxz,
314  bool with_type,
315  std::vector<std::string>& types)
316 {
317  return impl_->getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type, types);
318 }
319 
320 std::map<std::string, geometry_msgs::Pose>
321 PlanningSceneInterface::getObjectPoses(const std::vector<std::string>& object_ids)
322 {
323  return impl_->getObjectPoses(object_ids);
324 }
325 
326 std::map<std::string, moveit_msgs::CollisionObject>
327 PlanningSceneInterface::getObjects(const std::vector<std::string>& object_ids)
328 {
329  return impl_->getObjects(object_ids);
330 }
331 
332 std::map<std::string, moveit_msgs::AttachedCollisionObject>
333 PlanningSceneInterface::getAttachedObjects(const std::vector<std::string>& object_ids)
334 {
335  return impl_->getAttachedObjects(object_ids);
336 }
337 
338 bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::CollisionObject& collision_object)
339 {
340  moveit_msgs::PlanningScene ps;
341  ps.robot_state.is_diff = true;
342  ps.is_diff = true;
343  ps.world.collision_objects.reserve(1);
344  ps.world.collision_objects.push_back(collision_object);
345  return applyPlanningScene(ps);
346 }
347 
348 bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::CollisionObject& collision_object,
349  const std_msgs::ColorRGBA& object_color)
350 {
351  moveit_msgs::PlanningScene ps;
352  ps.robot_state.is_diff = true;
353  ps.is_diff = true;
354  ps.world.collision_objects.reserve(1);
355  ps.world.collision_objects.push_back(collision_object);
356  moveit_msgs::ObjectColor oc;
357  oc.id = collision_object.id;
358  oc.color = object_color;
359  ps.object_colors.push_back(oc);
360  return applyPlanningScene(ps);
361 }
362 
363 bool PlanningSceneInterface::applyCollisionObjects(const std::vector<moveit_msgs::CollisionObject>& collision_objects,
364  const std::vector<moveit_msgs::ObjectColor>& object_colors)
365 {
366  moveit_msgs::PlanningScene ps;
367  ps.robot_state.is_diff = true;
368  ps.is_diff = true;
369  ps.world.collision_objects = collision_objects;
370  ps.object_colors = object_colors;
371 
372  for (size_t i = 0; i < ps.object_colors.size(); ++i)
373  {
374  if (ps.object_colors[i].id.empty() && i < collision_objects.size())
375  ps.object_colors[i].id = collision_objects[i].id;
376  else
377  break;
378  }
379 
380  return applyPlanningScene(ps);
381 }
382 
383 bool PlanningSceneInterface::applyAttachedCollisionObject(const moveit_msgs::AttachedCollisionObject& collision_object)
384 {
385  moveit_msgs::PlanningScene ps;
386  ps.robot_state.is_diff = true;
387  ps.is_diff = true;
388  ps.robot_state.attached_collision_objects.reserve(1);
389  ps.robot_state.attached_collision_objects.push_back(collision_object);
390  return applyPlanningScene(ps);
391 }
392 
394  const std::vector<moveit_msgs::AttachedCollisionObject>& collision_objects)
395 {
396  moveit_msgs::PlanningScene ps;
397  ps.robot_state.is_diff = true;
398  ps.is_diff = true;
399  ps.robot_state.attached_collision_objects = collision_objects;
400  return applyPlanningScene(ps);
401 }
402 
403 bool PlanningSceneInterface::applyPlanningScene(const moveit_msgs::PlanningScene& ps)
404 {
405  return impl_->applyPlanningScene(ps);
406 }
407 
408 void PlanningSceneInterface::addCollisionObjects(const std::vector<moveit_msgs::CollisionObject>& collision_objects,
409  const std::vector<moveit_msgs::ObjectColor>& object_colors) const
410 {
411  impl_->addCollisionObjects(collision_objects, object_colors);
412 }
413 
414 void PlanningSceneInterface::removeCollisionObjects(const std::vector<std::string>& object_ids) const
415 {
416  impl_->removeCollisionObjects(object_ids);
417 }
418 } // namespace planning_interface
419 } // namespace moveit
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
std::map< std::string, geometry_msgs::Pose > getObjectPoses(const std::vector< std::string > &object_ids)
bool applyCollisionObject(const moveit_msgs::CollisionObject &collision_object)
Apply collision object to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group&#39;s monitored scene.
static const std::string GET_PLANNING_SCENE_SERVICE_NAME
#define ROS_WARN_NAMED(name,...)
bool applyCollisionObjects(const std::vector< moveit_msgs::CollisionObject > &collision_objects, const std::vector< moveit_msgs::ObjectColor > &object_colors=std::vector< moveit_msgs::ObjectColor >())
Apply collision objects to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group&#39;s monitored scene. If object_colors do not specify an id, the corresponding object id from collision_objects is used.
void wait(int seconds)
bool call(MReq &req, MRes &res)
std::map< std::string, moveit_msgs::CollisionObject > getObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the objects identified by the given object ids list. If no ids are provided, return all the known...
std::vector< std::string > getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz, bool with_type, std::vector< std::string > &types)
static const std::string APPLY_PLANNING_SCENE_SERVICE_NAME
std::map< std::string, geometry_msgs::Pose > getObjectPoses(const std::vector< std::string > &object_ids)
Get the poses from the objects identified by the given object ids list.
void publish(const boost::shared_ptr< M > &message) const
bool applyAttachedCollisionObjects(const std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objects)
Apply attached collision objects to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group&#39;s monitored scene.
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool applyPlanningScene(const moveit_msgs::PlanningScene &ps)
Update the planning_scene of the move_group node with the given ps synchronously. Other PlanningScene...
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
constexpr char LOGNAME[]
Definition: moveit_cpp.cpp:52
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
Remove collision objects from the world via /planning_scene.
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
std::map< std::string, moveit_msgs::AttachedCollisionObject > getAttachedObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the attached objects identified by the given object ids list. If no ids are provided, return all the attached objects.
bool applyAttachedCollisionObject(const moveit_msgs::AttachedCollisionObject &attached_collision_object)
Apply attached collision object to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group&#39;s monitored scene.
void addCollisionObjects(const std::vector< moveit_msgs::CollisionObject > &collision_objects, const std::vector< moveit_msgs::ObjectColor > &object_colors) const
std::vector< std::string > getKnownObjectNames(bool with_type=false)
Get the names of all known objects in the world. If with_type is set to true, only return objects tha...
std::vector< std::string > getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz, bool with_type, std::vector< std::string > &types)
Get the names of known objects in the world that are located within a bounding region (specified in t...
std::string getService()
PlanningSceneInterface(const std::string &ns="", bool wait=true)
std::map< std::string, moveit_msgs::AttachedCollisionObject > getAttachedObjects(const std::vector< std::string > &object_ids)
std::map< std::string, moveit_msgs::CollisionObject > getObjects(const std::vector< std::string > &object_ids)
const std::string response
void addCollisionObjects(const std::vector< moveit_msgs::CollisionObject > &collision_objects, const std::vector< moveit_msgs::ObjectColor > &object_colors=std::vector< moveit_msgs::ObjectColor >()) const
Add collision objects to the world via /planning_scene. Make sure object.operation is set to object...
#define ROS_WARN_STREAM_NAMED(name, args)


planning_interface
Author(s): Ioan Sucan
autogenerated on Fri Jun 5 2020 03:53:39