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 
50 class PlanningSceneInterface::PlanningSceneInterfaceImpl
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  {
66  }
67  else
68  {
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 (const moveit_msgs::CollisionObject& collision_object : response.scene.world.collision_objects)
87  if (!collision_object.type.key.empty())
88  result.push_back(collision_object.id);
89  }
90  else
91  {
92  for (const moveit_msgs::CollisionObject& collision_object : response.scene.world.collision_objects)
93  result.push_back(collision_object.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 (const moveit_msgs::CollisionObject& collision_object : response.scene.world.collision_objects)
112  {
113  if (with_type && collision_object.type.key.empty())
114  continue;
115  if (collision_object.mesh_poses.empty() && collision_object.primitive_poses.empty())
116  continue;
117  bool good = true;
118  for (const geometry_msgs::Pose& mesh_pose : collision_object.mesh_poses)
119  if (!(mesh_pose.position.x >= minx && mesh_pose.position.x <= maxx && mesh_pose.position.y >= miny &&
120  mesh_pose.position.y <= maxy && mesh_pose.position.z >= minz && mesh_pose.position.z <= maxz))
121  {
122  good = false;
123  break;
124  }
125  for (const geometry_msgs::Pose& primitive_pose : collision_object.primitive_poses)
126  if (!(primitive_pose.position.x >= minx && primitive_pose.position.x <= maxx &&
127  primitive_pose.position.y >= miny && primitive_pose.position.y <= maxy &&
128  primitive_pose.position.z >= minz && primitive_pose.position.z <= maxz))
129  {
130  good = false;
131  break;
132  }
133  if (good)
134  {
135  result.push_back(collision_object.id);
136  if (with_type)
137  types.push_back(collision_object.type.key);
138  }
139  }
140  return result;
141  }
142 
143  std::map<std::string, geometry_msgs::Pose> getObjectPoses(const std::vector<std::string>& object_ids)
144  {
145  moveit_msgs::GetPlanningScene::Request request;
146  moveit_msgs::GetPlanningScene::Response response;
147  std::map<std::string, geometry_msgs::Pose> result;
148  request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
149  if (!planning_scene_service_.call(request, response))
150  {
151  ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object names");
152  return result;
153  }
154 
155  for (const moveit_msgs::CollisionObject& collision_object : response.scene.world.collision_objects)
156  {
157  if (std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end())
158  {
159  if (collision_object.mesh_poses.empty() && collision_object.primitive_poses.empty())
160  continue;
161  if (!collision_object.mesh_poses.empty())
162  result[collision_object.id] = collision_object.mesh_poses[0];
163  else
164  result[collision_object.id] = collision_object.primitive_poses[0];
165  }
166  }
167  return result;
168  }
169 
170  std::map<std::string, moveit_msgs::CollisionObject> getObjects(const std::vector<std::string>& object_ids)
171  {
172  moveit_msgs::GetPlanningScene::Request request;
173  moveit_msgs::GetPlanningScene::Response response;
174  std::map<std::string, moveit_msgs::CollisionObject> result;
175  request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
176  if (!planning_scene_service_.call(request, response))
177  {
178  ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get object geometries");
179  return result;
180  }
181 
182  for (const moveit_msgs::CollisionObject& collision_object : response.scene.world.collision_objects)
183  {
184  if (object_ids.empty() || std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end())
185  {
186  result[collision_object.id] = collision_object;
187  }
188  }
189  return result;
190  }
191 
192  std::map<std::string, moveit_msgs::AttachedCollisionObject>
193  getAttachedObjects(const std::vector<std::string>& object_ids)
194  {
195  moveit_msgs::GetPlanningScene::Request request;
196  moveit_msgs::GetPlanningScene::Response response;
197  std::map<std::string, moveit_msgs::AttachedCollisionObject> result;
198  request.components.components = request.components.ROBOT_STATE_ATTACHED_OBJECTS;
199  if (!planning_scene_service_.call(request, response))
200  {
201  ROS_WARN_NAMED(LOGNAME, "Could not call planning scene service to get attached object geometries");
202  return result;
203  }
204 
205  for (const moveit_msgs::AttachedCollisionObject& attached_collision_object :
206  response.scene.robot_state.attached_collision_objects)
207  {
208  if (object_ids.empty() ||
209  std::find(object_ids.begin(), object_ids.end(), attached_collision_object.object.id) != object_ids.end())
210  {
211  result[attached_collision_object.object.id] = attached_collision_object;
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(LOGNAME, "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 
237  for (size_t i = 0; i < planning_scene.object_colors.size(); ++i)
238  {
239  if (planning_scene.object_colors[i].id.empty() && i < collision_objects.size())
240  planning_scene.object_colors[i].id = collision_objects[i].id;
241  else
242  break;
243  }
244 
245  planning_scene.is_diff = true;
247  }
248 
249  void removeCollisionObjects(const std::vector<std::string>& object_ids) const
250  {
251  moveit_msgs::PlanningScene planning_scene;
252  moveit_msgs::CollisionObject object;
253  for (const std::string& object_id : object_ids)
254  {
255  object.id = object_id;
256  object.operation = object.REMOVE;
257  planning_scene.world.collision_objects.push_back(object);
258  }
259  planning_scene.is_diff = true;
261  }
262 
263 private:
265  {
266  ros::Duration time_before_warning(5.0);
267  srv.waitForExistence(time_before_warning);
268  if (!srv.exists())
269  {
270  ROS_WARN_STREAM_NAMED(LOGNAME, "service '" << srv.getService() << "' not advertised yet. Continue waiting...");
271  srv.waitForExistence();
272  }
273  }
274 
279  moveit::core::RobotModelConstPtr robot_model_;
280 };
281 
282 PlanningSceneInterface::PlanningSceneInterface(const std::string& ns, bool wait)
283 {
285 }
286 
288 {
289  delete impl_;
290 }
291 
292 std::vector<std::string> PlanningSceneInterface::getKnownObjectNames(bool with_type)
293 {
294  return impl_->getKnownObjectNames(with_type);
295 }
296 
297 std::vector<std::string> PlanningSceneInterface::getKnownObjectNamesInROI(double minx, double miny, double minz,
298  double maxx, double maxy, double maxz,
299  bool with_type,
300  std::vector<std::string>& types)
301 {
302  return impl_->getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type, types);
303 }
304 
305 std::map<std::string, geometry_msgs::Pose>
306 PlanningSceneInterface::getObjectPoses(const std::vector<std::string>& object_ids)
307 {
308  return impl_->getObjectPoses(object_ids);
309 }
310 
311 std::map<std::string, moveit_msgs::CollisionObject>
312 PlanningSceneInterface::getObjects(const std::vector<std::string>& object_ids)
313 {
314  return impl_->getObjects(object_ids);
315 }
316 
317 std::map<std::string, moveit_msgs::AttachedCollisionObject>
318 PlanningSceneInterface::getAttachedObjects(const std::vector<std::string>& object_ids)
319 {
320  return impl_->getAttachedObjects(object_ids);
321 }
322 
323 bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::CollisionObject& collision_object)
324 {
325  moveit_msgs::PlanningScene ps;
326  ps.robot_state.is_diff = true;
327  ps.is_diff = true;
328  ps.world.collision_objects.reserve(1);
329  ps.world.collision_objects.push_back(collision_object);
330  return applyPlanningScene(ps);
331 }
332 
333 bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::CollisionObject& collision_object,
334  const std_msgs::ColorRGBA& object_color)
335 {
336  moveit_msgs::PlanningScene ps;
337  ps.robot_state.is_diff = true;
338  ps.is_diff = true;
339  ps.world.collision_objects.reserve(1);
340  ps.world.collision_objects.push_back(collision_object);
341  moveit_msgs::ObjectColor oc;
342  oc.id = collision_object.id;
343  oc.color = object_color;
344  ps.object_colors.push_back(oc);
345  return applyPlanningScene(ps);
346 }
347 
348 bool PlanningSceneInterface::applyCollisionObjects(const std::vector<moveit_msgs::CollisionObject>& collision_objects,
349  const std::vector<moveit_msgs::ObjectColor>& object_colors)
350 {
351  moveit_msgs::PlanningScene ps;
352  ps.robot_state.is_diff = true;
353  ps.is_diff = true;
354  ps.world.collision_objects = collision_objects;
355  ps.object_colors = object_colors;
356 
357  for (size_t i = 0; i < ps.object_colors.size(); ++i)
358  {
359  if (ps.object_colors[i].id.empty() && i < collision_objects.size())
360  ps.object_colors[i].id = collision_objects[i].id;
361  else
362  break;
363  }
364 
365  return applyPlanningScene(ps);
366 }
367 
368 bool PlanningSceneInterface::applyAttachedCollisionObject(const moveit_msgs::AttachedCollisionObject& collision_object)
369 {
370  moveit_msgs::PlanningScene ps;
371  ps.robot_state.is_diff = true;
372  ps.is_diff = true;
373  ps.robot_state.attached_collision_objects.reserve(1);
374  ps.robot_state.attached_collision_objects.push_back(collision_object);
375  return applyPlanningScene(ps);
376 }
377 
379  const std::vector<moveit_msgs::AttachedCollisionObject>& collision_objects)
380 {
381  moveit_msgs::PlanningScene ps;
382  ps.robot_state.is_diff = true;
383  ps.is_diff = true;
384  ps.robot_state.attached_collision_objects = collision_objects;
385  return applyPlanningScene(ps);
386 }
387 
388 bool PlanningSceneInterface::applyPlanningScene(const moveit_msgs::PlanningScene& ps)
389 {
390  return impl_->applyPlanningScene(ps);
391 }
392 
393 void PlanningSceneInterface::addCollisionObjects(const std::vector<moveit_msgs::CollisionObject>& collision_objects,
394  const std::vector<moveit_msgs::ObjectColor>& object_colors) const
395 {
396  impl_->addCollisionObjects(collision_objects, object_colors);
397 }
398 
399 void PlanningSceneInterface::removeCollisionObjects(const std::vector<std::string>& object_ids) const
400 {
401  impl_->removeCollisionObjects(object_ids);
402 }
403 } // namespace planning_interface
404 } // namespace moveit
response
const std::string response
move_group::GET_PLANNING_SCENE_SERVICE_NAME
static const std::string GET_PLANNING_SCENE_SERVICE_NAME
ros::ServiceClient::waitForExistence
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
move_group::APPLY_PLANNING_SCENE_SERVICE_NAME
static const std::string APPLY_PLANNING_SCENE_SERVICE_NAME
ros::Publisher
moveit::planning_interface::PlanningSceneInterface::applyAttachedCollisionObject
bool applyAttachedCollisionObject(const moveit_msgs::AttachedCollisionObject &attached_collision_object)
Apply attached collision object to the planning scene of the move_group node synchronously....
Definition: planning_scene_interface.cpp:432
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::robot_model_
moveit::core::RobotModelConstPtr robot_model_
Definition: planning_scene_interface.cpp:343
ros::ServiceClient::exists
bool exists()
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::PlanningSceneInterfaceImpl
PlanningSceneInterfaceImpl(const std::string &ns="", bool wait=true)
Definition: planning_scene_interface.cpp:117
ros.h
moveit::planning_interface::LOGNAME
const std::string LOGNAME
Definition: move_group_interface.cpp:148
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::getKnownObjectNamesInROI
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)
Definition: planning_scene_interface.cpp:162
moveit::planning_interface::PlanningSceneInterface::applyAttachedCollisionObjects
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....
Definition: planning_scene_interface.cpp:442
ros::NodeHandle::serviceClient
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::planning_scene_service_
ros::ServiceClient planning_scene_service_
Definition: planning_scene_interface.cpp:340
moveit::planning_interface::PlanningSceneInterface::getKnownObjectNames
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...
Definition: planning_scene_interface.cpp:356
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::waitForService
void waitForService(ros::ServiceClient &srv)
Definition: planning_scene_interface.cpp:328
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::addCollisionObjects
void addCollisionObjects(const std::vector< moveit_msgs::CollisionObject > &collision_objects, const std::vector< moveit_msgs::ObjectColor > &object_colors) const
Definition: planning_scene_interface.cpp:294
wait
void wait(int seconds)
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterface
PlanningSceneInterface(const std::string &ns="", bool wait=true)
Definition: planning_scene_interface.cpp:346
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
moveit::planning_interface::PlanningSceneInterface::applyPlanningScene
bool applyPlanningScene(const moveit_msgs::PlanningScene &ps)
Update the planning_scene of the move_group node with the given ps synchronously. Other PlanningScene...
Definition: planning_scene_interface.cpp:452
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::removeCollisionObjects
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
Definition: planning_scene_interface.cpp:313
moveit::planning_interface::PlanningSceneInterface::getObjectPoses
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.
Definition: planning_scene_interface.cpp:370
ros::ServiceClient::getService
std::string getService()
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl
Definition: planning_scene_interface.cpp:114
capability_names.h
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::getObjectPoses
std::map< std::string, geometry_msgs::Pose > getObjectPoses(const std::vector< std::string > &object_ids)
Definition: planning_scene_interface.cpp:207
moveit::planning_interface::PlanningSceneInterface::impl_
PlanningSceneInterfaceImpl * impl_
Definition: planning_scene_interface.h:212
moveit::planning_interface::PlanningSceneInterface::~PlanningSceneInterface
~PlanningSceneInterface()
Definition: planning_scene_interface.cpp:351
ros::ServiceClient
moveit::planning_interface::PlanningSceneInterface::removeCollisionObjects
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
Remove collision objects from the world via /planning_scene.
Definition: planning_scene_interface.cpp:463
planning_scene_interface.h
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::planning_scene_diff_publisher_
ros::Publisher planning_scene_diff_publisher_
Definition: planning_scene_interface.cpp:342
ROS_WARN_STREAM_NAMED
#define ROS_WARN_STREAM_NAMED(name, args)
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::node_handle_
ros::NodeHandle node_handle_
Definition: planning_scene_interface.cpp:339
moveit
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::applyPlanningScene
bool applyPlanningScene(const moveit_msgs::PlanningScene &planning_scene)
Definition: planning_scene_interface.cpp:281
moveit::planning_interface::PlanningSceneInterface::getObjects
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...
Definition: planning_scene_interface.cpp:376
moveit::planning_interface::PlanningSceneInterface::applyCollisionObjects
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....
Definition: planning_scene_interface.cpp:412
planning_interface
ros::ServiceClient::call
bool call(const MReq &req, MRes &resp, const std::string &service_md5sum)
ROS_WARN_NAMED
#define ROS_WARN_NAMED(name,...)
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::apply_planning_scene_service_
ros::ServiceClient apply_planning_scene_service_
Definition: planning_scene_interface.cpp:341
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::getObjects
std::map< std::string, moveit_msgs::CollisionObject > getObjects(const std::vector< std::string > &object_ids)
Definition: planning_scene_interface.cpp:234
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::getKnownObjectNames
std::vector< std::string > getKnownObjectNames(bool with_type)
Definition: planning_scene_interface.cpp:140
planning_scene
moveit::planning_interface::PlanningSceneInterface::getAttachedObjects
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,...
Definition: planning_scene_interface.cpp:382
ros::Duration
moveit::planning_interface::PlanningSceneInterface::applyCollisionObject
bool applyCollisionObject(const moveit_msgs::CollisionObject &collision_object)
Apply collision object to the planning scene of the move_group node synchronously....
Definition: planning_scene_interface.cpp:387
moveit::planning_interface::PlanningSceneInterface::PlanningSceneInterfaceImpl::getAttachedObjects
std::map< std::string, moveit_msgs::AttachedCollisionObject > getAttachedObjects(const std::vector< std::string > &object_ids)
Definition: planning_scene_interface.cpp:257
moveit::planning_interface::PlanningSceneInterface::addCollisionObjects
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....
Definition: planning_scene_interface.cpp:457
ros::NodeHandle
moveit::planning_interface::PlanningSceneInterface::getKnownObjectNamesInROI
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...
Definition: planning_scene_interface.cpp:361


planning_interface
Author(s): Ioan Sucan
autogenerated on Sat Dec 12 2020 03:27:08