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