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