test_planning_scene.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, 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 */
36 
37 #include <gtest/gtest.h>
41 #include <urdf_parser/urdf_parser.h>
42 #include <fstream>
43 #include <sstream>
44 #include <string>
45 #include <boost/filesystem/path.hpp>
46 #include <ros/package.h>
47 
50 
51 TEST(PlanningScene, LoadRestore)
52 {
53  urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
54  srdf::ModelSharedPtr srdf_model(new srdf::Model());
55  planning_scene::PlanningScene ps(urdf_model, srdf_model);
56  moveit_msgs::PlanningScene ps_msg;
57  ps.getPlanningSceneMsg(ps_msg);
58  EXPECT_EQ(ps.getName(), ps_msg.name);
59  EXPECT_EQ(ps.getRobotModel()->getName(), ps_msg.robot_model_name);
60  ps.setPlanningSceneMsg(ps_msg);
61  EXPECT_EQ(ps.getName(), ps_msg.name);
62  EXPECT_EQ(ps.getRobotModel()->getName(), ps_msg.robot_model_name);
63 }
64 
65 TEST(PlanningScene, LoadRestoreDiff)
66 {
67  urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
68  srdf::ModelSharedPtr srdf_model(new srdf::Model());
69  auto ps = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);
70 
71  collision_detection::World& world = *ps->getWorldNonConst();
72 
73  /* add one object to ps's world */
74  Eigen::Isometry3d id = Eigen::Isometry3d::Identity();
75  world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
76 
77  /* ps can be written to and set from message */
78  moveit_msgs::PlanningScene ps_msg;
79  ps_msg.robot_state.is_diff = true;
81  ps->getPlanningSceneMsg(ps_msg);
82  ps->setPlanningSceneMsg(ps_msg);
83  EXPECT_EQ(ps_msg.world.collision_objects.size(), 1u);
84  EXPECT_EQ(ps_msg.world.collision_objects[0].id, "sphere");
85  EXPECT_TRUE(world.hasObject("sphere"));
86 
87  /* test diff scene on top of ps */
88  planning_scene::PlanningScenePtr next = ps->diff();
89  /* world is inherited from ps */
90  EXPECT_TRUE(next->getWorld()->hasObject("sphere"));
91 
92  /* object in overlay is only added in overlay */
93  next->getWorldNonConst()->addToObject("sphere_in_next_only", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id);
94  EXPECT_EQ(next->getWorld()->size(), 2u);
95  EXPECT_EQ(ps->getWorld()->size(), 1u);
96 
97  /* the worlds used for collision detection contain one and two objects, respectively */
98  EXPECT_EQ(ps->getCollisionEnv()->getWorld()->size(), 1u);
99  EXPECT_EQ(ps->getCollisionEnvUnpadded()->getWorld()->size(), 1u);
100 
101  EXPECT_EQ(next->getCollisionEnv()->getWorld()->size(), 2u);
102  EXPECT_EQ(next->getCollisionEnvUnpadded()->getWorld()->size(), 2u);
103 
104  /* maintained diff contains only overlay object */
105  next->getPlanningSceneDiffMsg(ps_msg);
106  EXPECT_EQ(ps_msg.world.collision_objects.size(), 1u);
107 
108  /* copy ps to next and apply diff */
109  next->decoupleParent();
110  moveit_msgs::PlanningScene ps_msg2;
111 
112  /* diff is empty now */
113  next->getPlanningSceneDiffMsg(ps_msg2);
114  EXPECT_EQ(ps_msg2.world.collision_objects.size(), 0u);
115 
116  /* next's world contains both objects */
117  next->getPlanningSceneMsg(ps_msg);
118  EXPECT_EQ(ps_msg.world.collision_objects.size(), 2u);
119  ps->setPlanningSceneMsg(ps_msg);
120  EXPECT_EQ(ps->getWorld()->size(), 2u);
121  EXPECT_EQ(ps->getCollisionEnv()->getWorld()->size(), 2u);
122  EXPECT_EQ(ps->getCollisionEnvUnpadded()->getWorld()->size(), 2u);
123 }
124 
125 TEST(PlanningScene, MakeAttachedDiff)
126 {
127  urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
128  srdf::ModelSharedPtr srdf_model(new srdf::Model());
129  auto ps = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);
130 
131  /* add a single object to ps's world */
132  collision_detection::World& world = *ps->getWorldNonConst();
133  Eigen::Isometry3d id = Eigen::Isometry3d::Identity();
134  world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
135 
136  /* attach object in diff */
137  planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff();
138 
139  moveit_msgs::AttachedCollisionObject att_obj;
140  att_obj.link_name = "r_wrist_roll_link";
141  att_obj.object.operation = moveit_msgs::CollisionObject::ADD;
142  att_obj.object.id = "sphere";
143  attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj);
144 
145  /* object is not in world anymore */
146  EXPECT_EQ(attached_object_diff_scene->getWorld()->size(), 0u);
147  /* it became part of the robot state though */
148  EXPECT_TRUE(attached_object_diff_scene->getCurrentState().hasAttachedBody("sphere"));
149 
152  attached_object_diff_scene->checkCollision(req, res);
153  ps->checkCollision(req, res);
154 }
155 
156 TEST(PlanningScene, isStateValid)
157 {
158  moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2");
159  auto ps = std::make_shared<planning_scene::PlanningScene>(robot_model->getURDF(), robot_model->getSRDF());
160  moveit::core::RobotState current_state = ps->getCurrentState();
161  if (ps->isStateColliding(current_state, "left_arm"))
162  {
163  EXPECT_FALSE(ps->isStateValid(current_state, "left_arm"));
164  }
165 }
166 
167 TEST(PlanningScene, loadGoodSceneGeometryNewFormat)
168 {
169  moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2");
170  auto ps = std::make_shared<planning_scene::PlanningScene>(robot_model->getURDF(), robot_model->getSRDF());
171 
172  std::istringstream good_scene_geometry;
173  good_scene_geometry.str("foobar_scene\n"
174  "* foo\n"
175  "0 0 0\n"
176  "0 0 0 1\n"
177  "1\n"
178  "box\n"
179  "2.58 1.36 0.31\n"
180  "1.49257 1.00222 0.170051\n"
181  "0 0 4.16377e-05 1\n"
182  "0 0 1 0.3\n"
183  "0\n"
184  "* bar\n"
185  "0 0 0\n"
186  "0 0 0 1\n"
187  "1\n"
188  "cylinder\n"
189  "0.02 0.0001\n"
190  "0.453709 0.499136 0.355051\n"
191  "0 0 4.16377e-05 1\n"
192  "1 0 0 1\n"
193  "0\n"
194  ".\n");
195  EXPECT_TRUE(ps->loadGeometryFromStream(good_scene_geometry));
196  EXPECT_EQ(ps->getName(), "foobar_scene");
197  EXPECT_TRUE(ps->getWorld()->hasObject("foo"));
198  EXPECT_TRUE(ps->getWorld()->hasObject("bar"));
199  EXPECT_FALSE(ps->getWorld()->hasObject("baz")); // Sanity check.
200 }
201 
202 TEST(PlanningScene, loadGoodSceneGeometryOldFormat)
203 {
204  moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2");
205  auto ps = std::make_shared<planning_scene::PlanningScene>(robot_model->getURDF(), robot_model->getSRDF());
206 
207  std::istringstream good_scene_geometry;
208  good_scene_geometry.str("foobar_scene\n"
209  "* foo\n"
210  "2\n"
211  "box\n"
212  ".77 0.39 0.05\n"
213  "0 0 0.025\n"
214  "0 0 0 1\n"
215  "0.82 0.75 0.60 1\n"
216  "box\n"
217  ".77 0.39 0.05\n"
218  "0 0 1.445\n"
219  "0 0 0 1\n"
220  "0.82 0.75 0.60 1\n"
221  ".\n");
222  EXPECT_TRUE(ps->loadGeometryFromStream(good_scene_geometry));
223  EXPECT_EQ(ps->getName(), "foobar_scene");
224  EXPECT_TRUE(ps->getWorld()->hasObject("foo"));
225  EXPECT_FALSE(ps->getWorld()->hasObject("baz")); // Sanity check.
226 }
227 
228 TEST(PlanningScene, loadBadSceneGeometry)
229 {
230  moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2");
231  auto ps = std::make_shared<planning_scene::PlanningScene>(robot_model->getURDF(), robot_model->getSRDF());
232  std::istringstream empty_scene_geometry;
233 
234  // This should fail since there is no planning scene name and no end of geometry marker.
235  EXPECT_FALSE(ps->loadGeometryFromStream(empty_scene_geometry));
236 
237  std::istringstream malformed_scene_geometry;
238  malformed_scene_geometry.str("malformed_scene_geometry\n"
239  "* foo\n"
240  "0 0 0\n"
241  "0 0 0 1\n"
242  "1\n"
243  "box\n"
244  "2.58 1.36\n" /* Only two tokens; should be 3 */
245  "1.49257 1.00222 0.170051\n"
246  "0 0 4.16377e-05 1\n"
247  "0 0 1 0.3\n"
248  ".\n");
249  EXPECT_FALSE(ps->loadGeometryFromStream(malformed_scene_geometry));
250 }
251 
252 TEST(PlanningScene, FailRetrievingNonExistentObject)
253 {
254  moveit::core::RobotModelPtr robot_model = moveit::core::loadTestingRobotModel("pr2");
256  moveit_msgs::CollisionObject obj;
257  EXPECT_FALSE(ps.getCollisionObjectMsg(obj, "non_existent_object"));
258 }
259 
260 class CollisionDetectorTests : public testing::TestWithParam<const char*>
261 {
262 };
264 {
265  const std::string plugin_name = GetParam();
266  SCOPED_TRACE(plugin_name);
267 
268  urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
269  srdf::ModelSharedPtr srdf_model(new srdf::Model());
270  // create parent scene
271  planning_scene::PlanningScenePtr parent = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);
272 
274  if (!loader.activate(plugin_name, parent, true))
275  {
276 #if defined(GTEST_SKIP_)
277  GTEST_SKIP_("Failed to load collision plugin");
278 #else
279  return;
280 #endif
281  }
282 
283  // create child scene
284  planning_scene::PlanningScenePtr child = parent->diff();
285 
286  // create collision request variables
289  moveit::core::RobotState* state = new moveit::core::RobotState(child->getRobotModel());
290  state->setToDefaultValues();
291  state->update();
292 
293  // there should be no collision with the environment
294  res.clear();
295  parent->getCollisionEnv()->checkRobotCollision(req, res, *state, parent->getAllowedCollisionMatrix());
296  EXPECT_FALSE(res.collision);
297  res.clear();
298  child->getCollisionEnv()->checkRobotCollision(req, res, *state, child->getAllowedCollisionMatrix());
299  EXPECT_FALSE(res.collision);
300 
301  // create message to add a collision object at the world origin
302  moveit_msgs::PlanningScene ps_msg;
303  ps_msg.is_diff = false;
304  moveit_msgs::CollisionObject co;
305  co.header.frame_id = "base_link";
306  co.operation = moveit_msgs::CollisionObject::ADD;
307  co.id = "box";
308  co.pose.orientation.w = 1.0;
309  {
310  shape_msgs::SolidPrimitive sp;
311  sp.type = shape_msgs::SolidPrimitive::BOX;
312  sp.dimensions = { 1., 1., 1. };
313  co.primitives.push_back(sp);
314  geometry_msgs::Pose sp_pose;
315  sp_pose.orientation.w = 1.0;
316  co.primitive_poses.push_back(sp_pose);
317  }
318  ps_msg.world.collision_objects.push_back(co);
319 
320  // add object to the parent planning scene
321  parent->usePlanningSceneMsg(ps_msg);
322 
323  // the parent scene should be in collision
324  res.clear();
325  parent->getCollisionEnv()->checkRobotCollision(req, res, *state, parent->getAllowedCollisionMatrix());
326  EXPECT_TRUE(res.collision);
327 
328  // the child scene was not updated yet, so no collision
329  res.clear();
330  child->getCollisionEnv()->checkRobotCollision(req, res, *state, child->getAllowedCollisionMatrix());
331  EXPECT_FALSE(res.collision);
332 
333  // update the child scene
334  child->clearDiffs();
335 
336  // child and parent scene should be in collision
337  res.clear();
338  parent->getCollisionEnv()->checkRobotCollision(req, res, *state, parent->getAllowedCollisionMatrix());
339  EXPECT_TRUE(res.collision);
340  res.clear();
341  child->getCollisionEnv()->checkRobotCollision(req, res, *state, child->getAllowedCollisionMatrix());
342  EXPECT_TRUE(res.collision);
343 
344  child.reset();
345  parent.reset();
346 }
347 
348 // Returns a planning scene diff message
349 moveit_msgs::PlanningScene create_planning_scene_diff(const planning_scene::PlanningScene& ps,
350  const std::string& object_name, const int8_t operation,
351  const bool attach_object = false, const bool create_object = true)
352 {
353  // Helper function to create an object for RobotStateDiffBug
354  auto add_object = [](const std::string& object_name, const int8_t operation) {
355  moveit_msgs::CollisionObject co;
356  co.header.frame_id = "panda_link0";
357  co.id = object_name;
358  co.operation = operation;
359  co.primitives.push_back([] {
360  shape_msgs::SolidPrimitive primitive;
361  primitive.type = shape_msgs::SolidPrimitive::SPHERE;
362  primitive.dimensions.push_back(1.0);
363  return primitive;
364  }());
365  co.primitive_poses.push_back([] {
366  geometry_msgs::Pose pose;
367  pose.orientation.w = 1.0;
368  return pose;
369  }());
370  co.pose = co.primitive_poses[0];
371  return co;
372  };
373  // Helper function to create an attached object for RobotStateDiffBug
374  auto add_attached_object = [](const std::string& object_name, const int8_t operation) {
375  moveit_msgs::AttachedCollisionObject aco;
376  aco.object.operation = operation;
377  aco.object.id = object_name;
378  aco.link_name = "panda_link0";
379  return aco;
380  };
381 
382  auto new_ps = ps.diff();
383  if ((operation == moveit_msgs::CollisionObject::REMOVE && !attach_object) ||
384  (operation == moveit_msgs::CollisionObject::ADD && create_object))
385  new_ps->processCollisionObjectMsg(add_object(object_name, operation));
386  if (attach_object)
387  new_ps->processAttachedCollisionObjectMsg(add_attached_object(object_name, operation));
388  moveit_msgs::PlanningScene scene_msg;
389  new_ps->getPlanningSceneDiffMsg(scene_msg);
390  return scene_msg;
391 }
392 
393 // Returns collision objects names sorted alphabetically
395 {
396  std::vector<moveit_msgs::CollisionObject> collision_objects;
397  ps.getCollisionObjectMsgs(collision_objects);
398  std::set<std::string> collision_objects_names;
399  for (const auto& collision_object : collision_objects)
400  collision_objects_names.emplace(collision_object.id);
401  return collision_objects_names;
402 }
403 
404 // Returns attached collision objects names sorted alphabetically
406 {
407  std::vector<moveit_msgs::AttachedCollisionObject> collision_objects;
408  ps.getAttachedCollisionObjectMsgs(collision_objects);
409  std::set<std::string> collision_objects_names;
410  for (const auto& collision_object : collision_objects)
411  collision_objects_names.emplace(collision_object.object.id);
412  return collision_objects_names;
413 }
414 
415 TEST(PlanningScene, RobotStateDiffBug)
416 {
417  auto urdf_model = moveit::core::loadModelInterface("panda");
418  auto srdf_model = std::make_shared<srdf::Model>();
419  auto ps = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);
420 
421  // Adding collision objects incrementally
422  {
423  const auto ps1 = create_planning_scene_diff(*ps, "object1", moveit_msgs::CollisionObject::ADD);
424  const auto ps2 = create_planning_scene_diff(*ps, "object2", moveit_msgs::CollisionObject::ADD);
425 
426  ps->usePlanningSceneMsg(ps1);
427  ps->usePlanningSceneMsg(ps2);
428  EXPECT_EQ(get_collision_objects_names(*ps), (std::set<std::string>{ "object1", "object2" }));
429  }
430 
431  // Removing a collision object
432  {
433  const auto ps1 = create_planning_scene_diff(*ps, "object2", moveit_msgs::CollisionObject::REMOVE);
434 
435  ps->usePlanningSceneMsg(ps1);
436  EXPECT_EQ(get_collision_objects_names(*ps), (std::set<std::string>{ "object1" }));
437  }
438 
439  // Adding attached collision objects incrementally
440  ps = std::make_shared<planning_scene::PlanningScene>(urdf_model, srdf_model);
441  {
442  const auto ps1 = create_planning_scene_diff(*ps, "object1", moveit_msgs::CollisionObject::ADD, true);
443  const auto ps2 = create_planning_scene_diff(*ps, "object2", moveit_msgs::CollisionObject::ADD, true);
444 
445  ps->usePlanningSceneMsg(ps1);
446  ps->usePlanningSceneMsg(ps2);
448  EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set<std::string>{ "object1", "object2" }));
449  }
450 
451  // Removing an attached collision object
452  {
453  const auto ps1 = create_planning_scene_diff(*ps, "object2", moveit_msgs::CollisionObject::REMOVE, true);
454  ps->usePlanningSceneMsg(ps1);
455 
456  EXPECT_EQ(get_collision_objects_names(*ps), (std::set<std::string>{ "object2" }));
457  EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set<std::string>{ "object1" }));
458  }
459 
460  // Turn an existing collision object into an attached object
461  {
462  const auto ps1 = create_planning_scene_diff(*ps, "object2", moveit_msgs::CollisionObject::ADD, true, false);
463  ps->usePlanningSceneMsg(ps1);
464 
466  EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set<std::string>{ "object1", "object2" }));
467  }
468 
469  // Removing an attached collision object completely
470  {
471  auto ps1 = ps->diff();
472  moveit_msgs::CollisionObject co;
473  co.id = "object2";
474  co.operation = moveit_msgs::CollisionObject::REMOVE;
475  moveit_msgs::AttachedCollisionObject aco;
476  aco.object = co;
477 
478  ps1->processAttachedCollisionObjectMsg(aco); // detach
479  ps1->processCollisionObjectMsg(co); // and eventually remove object
480 
481  moveit_msgs::PlanningScene msg;
482  ps1->getPlanningSceneDiffMsg(msg);
483  ps->usePlanningSceneMsg(msg);
484 
486  EXPECT_EQ(get_attached_collision_objects_names(*ps), (std::set<std::string>{ "object1" }));
487  }
488 }
489 
490 #ifndef INSTANTIATE_TEST_SUITE_P // prior to gtest 1.10
491 #define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
492 #endif
493 
494 // instantiate parameterized tests for common collision plugins
495 INSTANTIATE_TEST_SUITE_P(PluginTests, CollisionDetectorTests, testing::Values("FCL", "Bullet"));
496 
497 int main(int argc, char** argv)
498 {
499  testing::InitGoogleTest(&argc, argv);
500  return RUN_ALL_TESTS();
501 }
get_collision_objects_names
std::set< std::string > get_collision_objects_names(const planning_scene::PlanningScene &ps)
Definition: test_planning_scene.cpp:394
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
collision_detection::World::hasObject
bool hasObject(const std::string &object_id) const
Check if a particular object exists in the collision world.
Definition: world.cpp:155
main
int main(int argc, char **argv)
Definition: test_planning_scene.cpp:497
planning_scene::PlanningScene::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
Definition: planning_scene.h:174
planning_scene::PlanningScene::getCollisionObjectMsgs
void getCollisionObjectMsgs(std::vector< moveit_msgs::CollisionObject > &collision_objs) const
Construct a vector of messages (collision_objects) with the collision object data for all objects in ...
Definition: planning_scene.cpp:835
SCOPED_TRACE
#define SCOPED_TRACE(message)
srdf::ModelSharedPtr
std::shared_ptr< Model > ModelSharedPtr
planning_scene.h
collision_detection::World
Maintain a representation of the environment.
Definition: world.h:59
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
obj
CollisionObject< S > * obj
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
EXPECT_TRUE
#define EXPECT_TRUE(condition)
moveit::core::loadModelInterface
urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string &robot_name)
Loads a URDF Model Interface from moveit_resources.
Definition: robot_model_test_utils.cpp:124
planning_scene::PlanningScene::getPlanningSceneMsg
void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
Definition: planning_scene.cpp:907
collision_detection::CollisionPluginCache
Definition: collision_plugin_cache.h:75
planning_scene::PlanningScene::getAttachedCollisionObjectMsgs
void getAttachedCollisionObjectMsgs(std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objs) const
Construct a vector of messages (attached_collision_objects) with the attached collision object data f...
Definition: planning_scene.cpp:863
collision_common.h
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
planning_scene::PlanningScene
This class maintains the representation of the environment as seen by a planning instance....
Definition: planning_scene.h:122
collision_detection::World::addToObject
void addToObject(const std::string &object_id, const Eigen::Isometry3d &pose, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &shape_poses)
Add a pose and shapes to an object in the map. This function makes repeated calls to addToObjectInter...
Definition: world.cpp:100
moveit::core::RobotState::update
void update(bool force=false)
Update all transforms.
Definition: robot_state.cpp:720
collision_detection::CollisionPluginCache::activate
bool activate(const std::string &name, const planning_scene::PlanningScenePtr &scene, bool exclusive)
Activate a specific collision plugin for the given planning scene instance.
Definition: collision_plugin_cache.cpp:105
shapes::Sphere
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
get_attached_collision_objects_names
std::set< std::string > get_attached_collision_objects_names(const planning_scene::PlanningScene &ps)
Definition: test_planning_scene.cpp:405
EXPECT_FALSE
#define EXPECT_FALSE(condition)
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Definition: robot_state.cpp:425
collision_plugin_cache.h
next
EndPoint * next[3]
package.h
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
planning_scene::PlanningScene::getName
const std::string & getName() const
Get the name of the planning scene. This is empty by default.
Definition: planning_scene.h:140
robot_model_test_utils.h
planning_scene::PlanningScene::setPlanningSceneMsg
bool setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Set this instance of a planning scene to be the same as the one serialized in the scene message,...
Definition: planning_scene.cpp:1342
create_planning_scene_diff
moveit_msgs::PlanningScene create_planning_scene_diff(const planning_scene::PlanningScene &ps, const std::string &object_name, const int8_t operation, const bool attach_object=false, const bool create_object=true)
Definition: test_planning_scene.cpp:349
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
collision_detection::CollisionResult::collision
bool collision
True if collision was found, false otherwise.
Definition: include/moveit/collision_detection/collision_common.h:200
gtest.h
CollisionDetectorTests
Definition: test_multi_threaded.cpp:69
collision_detection::CollisionResult::clear
EIGEN_MAKE_ALIGNED_OPERATOR_NEW void clear()
Clear a previously stored result.
Definition: include/moveit/collision_detection/collision_common.h:187
srdf::Model
message_checks.h
planning_scene::PlanningScene::diff
PlanningScenePtr diff() const
Return a new child PlanningScene that uses this one as parent.
Definition: planning_scene.cpp:246
TEST_P
TEST_P(CollisionDetectorTests, ClearDiff)
Definition: test_planning_scene.cpp:263
moveit::core::loadTestingRobotModel
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
Definition: robot_model_test_utils.cpp:116
INSTANTIATE_TEST_SUITE_P
#define INSTANTIATE_TEST_SUITE_P(...)
Definition: test_planning_scene.cpp:491
TEST
TEST(PlanningScene, LoadRestore)
Definition: test_planning_scene.cpp:51
moveit::core::isEmpty
bool isEmpty(const moveit_msgs::PlanningScene &msg)
Check if a message includes any information about a planning scene, or whether it is empty.
Definition: message_checks.cpp:107


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Aug 3 2022 02:24:18