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 {
49 {
50 public:
51  explicit PlanningSceneInterfaceImpl(const std::string& ns = "")
52  {
58  planning_scene_diff_publisher_ = node_handle_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
59  }
60 
61  std::vector<std::string> getKnownObjectNames(bool with_type)
62  {
63  moveit_msgs::GetPlanningScene::Request request;
64  moveit_msgs::GetPlanningScene::Response response;
65  std::vector<std::string> result;
66  request.components.components = request.components.WORLD_OBJECT_NAMES;
67  if (!planning_scene_service_.call(request, response))
68  return result;
69  if (with_type)
70  {
71  for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
72  if (!response.scene.world.collision_objects[i].type.key.empty())
73  result.push_back(response.scene.world.collision_objects[i].id);
74  }
75  else
76  {
77  for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
78  result.push_back(response.scene.world.collision_objects[i].id);
79  }
80  return result;
81  }
82 
83  std::vector<std::string> getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy,
84  double maxz, bool with_type, std::vector<std::string>& types)
85  {
86  moveit_msgs::GetPlanningScene::Request request;
87  moveit_msgs::GetPlanningScene::Response response;
88  std::vector<std::string> result;
89  request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
90  if (!planning_scene_service_.call(request, response))
91  {
92  ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object names");
93  return result;
94  }
95 
96  for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
97  {
98  if (with_type && response.scene.world.collision_objects[i].type.key.empty())
99  continue;
100  if (response.scene.world.collision_objects[i].mesh_poses.empty() &&
101  response.scene.world.collision_objects[i].primitive_poses.empty())
102  continue;
103  bool good = true;
104  for (std::size_t j = 0; j < response.scene.world.collision_objects[i].mesh_poses.size(); ++j)
105  if (!(response.scene.world.collision_objects[i].mesh_poses[j].position.x >= minx &&
106  response.scene.world.collision_objects[i].mesh_poses[j].position.x <= maxx &&
107  response.scene.world.collision_objects[i].mesh_poses[j].position.y >= miny &&
108  response.scene.world.collision_objects[i].mesh_poses[j].position.y <= maxy &&
109  response.scene.world.collision_objects[i].mesh_poses[j].position.z >= minz &&
110  response.scene.world.collision_objects[i].mesh_poses[j].position.z <= maxz))
111  {
112  good = false;
113  break;
114  }
115  for (std::size_t j = 0; j < response.scene.world.collision_objects[i].primitive_poses.size(); ++j)
116  if (!(response.scene.world.collision_objects[i].primitive_poses[j].position.x >= minx &&
117  response.scene.world.collision_objects[i].primitive_poses[j].position.x <= maxx &&
118  response.scene.world.collision_objects[i].primitive_poses[j].position.y >= miny &&
119  response.scene.world.collision_objects[i].primitive_poses[j].position.y <= maxy &&
120  response.scene.world.collision_objects[i].primitive_poses[j].position.z >= minz &&
121  response.scene.world.collision_objects[i].primitive_poses[j].position.z <= maxz))
122  {
123  good = false;
124  break;
125  }
126  if (good)
127  {
128  result.push_back(response.scene.world.collision_objects[i].id);
129  if (with_type)
130  types.push_back(response.scene.world.collision_objects[i].type.key);
131  }
132  }
133  return result;
134  }
135 
136  std::map<std::string, geometry_msgs::Pose> getObjectPoses(const std::vector<std::string>& object_ids)
137  {
138  moveit_msgs::GetPlanningScene::Request request;
139  moveit_msgs::GetPlanningScene::Response response;
140  std::map<std::string, geometry_msgs::Pose> result;
141  request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
142  if (!planning_scene_service_.call(request, response))
143  {
144  ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object names");
145  return result;
146  }
147 
148  for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
149  {
150  if (std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) !=
151  object_ids.end())
152  {
153  if (response.scene.world.collision_objects[i].mesh_poses.empty() &&
154  response.scene.world.collision_objects[i].primitive_poses.empty())
155  continue;
156  if (!response.scene.world.collision_objects[i].mesh_poses.empty())
157  result[response.scene.world.collision_objects[i].id] =
158  response.scene.world.collision_objects[i].mesh_poses[0];
159  else
160  result[response.scene.world.collision_objects[i].id] =
161  response.scene.world.collision_objects[i].primitive_poses[0];
162  }
163  }
164  return result;
165  }
166 
167  std::map<std::string, moveit_msgs::CollisionObject> getObjects(const std::vector<std::string>& object_ids)
168  {
169  moveit_msgs::GetPlanningScene::Request request;
170  moveit_msgs::GetPlanningScene::Response response;
171  std::map<std::string, moveit_msgs::CollisionObject> result;
172  request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
173  if (!planning_scene_service_.call(request, response))
174  {
175  ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object geometries");
176  return result;
177  }
178 
179  for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
180  {
181  if (object_ids.empty() ||
182  std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) !=
183  object_ids.end())
184  {
185  result[response.scene.world.collision_objects[i].id] = response.scene.world.collision_objects[i];
186  }
187  }
188  return result;
189  }
190 
191  std::map<std::string, moveit_msgs::AttachedCollisionObject>
192  getAttachedObjects(const std::vector<std::string>& object_ids)
193  {
194  moveit_msgs::GetPlanningScene::Request request;
195  moveit_msgs::GetPlanningScene::Response response;
196  std::map<std::string, moveit_msgs::AttachedCollisionObject> result;
197  request.components.components = request.components.ROBOT_STATE_ATTACHED_OBJECTS;
198  if (!planning_scene_service_.call(request, response))
199  {
200  ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get attached object "
201  "geometries");
202  return result;
203  }
204 
205  for (std::size_t i = 0; i < response.scene.robot_state.attached_collision_objects.size(); ++i)
206  {
207  if (object_ids.empty() ||
208  std::find(object_ids.begin(), object_ids.end(),
209  response.scene.robot_state.attached_collision_objects[i].object.id) != object_ids.end())
210  {
211  result[response.scene.robot_state.attached_collision_objects[i].object.id] =
212  response.scene.robot_state.attached_collision_objects[i];
213  }
214  }
215  return result;
216  }
217 
218  bool applyPlanningScene(const moveit_msgs::PlanningScene& planning_scene)
219  {
220  moveit_msgs::ApplyPlanningScene::Request request;
221  moveit_msgs::ApplyPlanningScene::Response response;
222  request.scene = planning_scene;
223  if (!apply_planning_scene_service_.call(request, response))
224  {
225  ROS_WARN_NAMED("planning_scene_interface", "Failed to call ApplyPlanningScene service");
226  return false;
227  }
228  return response.success;
229  }
230 
231  void addCollisionObjects(const std::vector<moveit_msgs::CollisionObject>& collision_objects,
232  const std::vector<moveit_msgs::ObjectColor>& object_colors) const
233  {
234  moveit_msgs::PlanningScene planning_scene;
235  planning_scene.world.collision_objects = collision_objects;
236  planning_scene.object_colors = object_colors;
237 
238  for (size_t i = 0; i < planning_scene.object_colors.size(); ++i)
239  {
240  if (planning_scene.object_colors[i].id.empty() && i < collision_objects.size())
241  planning_scene.object_colors[i].id = collision_objects[i].id;
242  else
243  break;
244  }
245 
246  planning_scene.is_diff = true;
247  planning_scene_diff_publisher_.publish(planning_scene);
248  }
249 
250  void removeCollisionObjects(const std::vector<std::string>& object_ids) const
251  {
252  moveit_msgs::PlanningScene planning_scene;
253  moveit_msgs::CollisionObject object;
254  for (std::size_t i = 0; i < object_ids.size(); ++i)
255  {
256  object.id = object_ids[i];
257  object.operation = object.REMOVE;
258  planning_scene.world.collision_objects.push_back(object);
259  }
260  planning_scene.is_diff = true;
261  planning_scene_diff_publisher_.publish(planning_scene);
262  }
263 
264 private:
269  robot_model::RobotModelConstPtr robot_model_;
270 };
271 
273 {
275 }
276 
278 {
279  delete impl_;
280 }
281 
282 std::vector<std::string> PlanningSceneInterface::getKnownObjectNames(bool with_type)
283 {
284  return impl_->getKnownObjectNames(with_type);
285 }
286 
287 std::vector<std::string> PlanningSceneInterface::getKnownObjectNamesInROI(double minx, double miny, double minz,
288  double maxx, double maxy, double maxz,
289  bool with_type,
290  std::vector<std::string>& types)
291 {
292  return impl_->getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type, types);
293 }
294 
295 std::map<std::string, geometry_msgs::Pose>
296 PlanningSceneInterface::getObjectPoses(const std::vector<std::string>& object_ids)
297 {
298  return impl_->getObjectPoses(object_ids);
299 }
300 
301 std::map<std::string, moveit_msgs::CollisionObject>
302 PlanningSceneInterface::getObjects(const std::vector<std::string>& object_ids)
303 {
304  return impl_->getObjects(object_ids);
305 }
306 
307 std::map<std::string, moveit_msgs::AttachedCollisionObject>
308 PlanningSceneInterface::getAttachedObjects(const std::vector<std::string>& object_ids)
309 {
310  return impl_->getAttachedObjects(object_ids);
311 }
312 
313 bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::CollisionObject& collision_object)
314 {
315  moveit_msgs::PlanningScene ps;
316  ps.robot_state.is_diff = true;
317  ps.is_diff = true;
318  ps.world.collision_objects.reserve(1);
319  ps.world.collision_objects.push_back(collision_object);
320  return applyPlanningScene(ps);
321 }
322 
323 bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::CollisionObject& collision_object,
324  const std_msgs::ColorRGBA& object_color)
325 {
326  moveit_msgs::PlanningScene ps;
327  ps.robot_state.is_diff = true;
328  ps.is_diff = true;
329  ps.world.collision_objects.reserve(1);
330  ps.world.collision_objects.push_back(collision_object);
331  moveit_msgs::ObjectColor oc;
332  oc.id = collision_object.id;
333  oc.color = object_color;
334  ps.object_colors.push_back(oc);
335  return applyPlanningScene(ps);
336 }
337 
338 bool PlanningSceneInterface::applyCollisionObjects(const std::vector<moveit_msgs::CollisionObject>& collision_objects,
339  const std::vector<moveit_msgs::ObjectColor>& object_colors)
340 {
341  moveit_msgs::PlanningScene ps;
342  ps.robot_state.is_diff = true;
343  ps.is_diff = true;
344  ps.world.collision_objects = collision_objects;
345  ps.object_colors = object_colors;
346 
347  for (size_t i = 0; i < ps.object_colors.size(); ++i)
348  {
349  if (ps.object_colors[i].id.empty() && i < collision_objects.size())
350  ps.object_colors[i].id = collision_objects[i].id;
351  else
352  break;
353  }
354 
355  return applyPlanningScene(ps);
356 }
357 
358 bool PlanningSceneInterface::applyAttachedCollisionObject(const moveit_msgs::AttachedCollisionObject& collision_object)
359 {
360  moveit_msgs::PlanningScene ps;
361  ps.robot_state.is_diff = true;
362  ps.is_diff = true;
363  ps.robot_state.attached_collision_objects.reserve(1);
364  ps.robot_state.attached_collision_objects.push_back(collision_object);
365  return applyPlanningScene(ps);
366 }
367 
369  const std::vector<moveit_msgs::AttachedCollisionObject>& collision_objects)
370 {
371  moveit_msgs::PlanningScene ps;
372  ps.robot_state.is_diff = true;
373  ps.is_diff = true;
374  ps.robot_state.attached_collision_objects = collision_objects;
375  return applyPlanningScene(ps);
376 }
377 
378 bool PlanningSceneInterface::applyPlanningScene(const moveit_msgs::PlanningScene& ps)
379 {
380  return impl_->applyPlanningScene(ps);
381 }
382 
383 void PlanningSceneInterface::addCollisionObjects(const std::vector<moveit_msgs::CollisionObject>& collision_objects,
384  const std::vector<moveit_msgs::ObjectColor>& object_colors) const
385 {
386  impl_->addCollisionObjects(collision_objects, object_colors);
387 }
388 
389 void PlanningSceneInterface::removeCollisionObjects(const std::vector<std::string>& object_ids) const
390 {
391  impl_->removeCollisionObjects(object_ids);
392 }
393 }
394 }
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
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
void publish(const boost::shared_ptr< M > &message) const
#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.
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...
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...
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.
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 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)
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.
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...
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
Remove collision objects from the world via /planning_scene.
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...
void addCollisionObjects(const std::vector< moveit_msgs::CollisionObject > &collision_objects, const std::vector< moveit_msgs::ObjectColor > &object_colors) const
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)


planning_interface
Author(s): Ioan Sucan
autogenerated on Mon Jul 16 2018 19:29:14