moveit2
The MoveIt Motion Planning Framework for ROS 2.
test_world.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: Acorn Pooley */
36 
37 #include <gtest/gtest.h>
39 #include <geometric_shapes/shapes.h>
40 #include <functional>
41 
42 using namespace collision_detection;
43 
44 TEST(World, AddRemoveShape)
45 {
46  World world;
47 
48  // Create some shapes
49  shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
50  shapes::ShapePtr box = std::make_shared<shapes::Box>(1, 2, 3);
51  shapes::ShapePtr cyl = std::make_shared<shapes::Cylinder>(4, 5);
52 
53  EXPECT_EQ(1, ball.use_count());
54 
55  EXPECT_FALSE(world.hasObject("ball"));
56 
57  // Add ball object
58  world.addToObject("ball", ball, Eigen::Isometry3d::Identity());
59 
60  EXPECT_EQ(2, ball.use_count());
61  EXPECT_TRUE(world.hasObject("ball"));
62 
63  bool move_ok = world.moveShapeInObject("ball", ball, Eigen::Isometry3d(Eigen::Translation3d(0, 0, 9)));
64  EXPECT_TRUE(move_ok);
65 
66  EXPECT_EQ(2, ball.use_count());
67  EXPECT_TRUE(world.hasObject("ball"));
68 
69  bool rm_nonexistant = world.removeShapeFromObject("xyz", ball);
70  EXPECT_FALSE(rm_nonexistant);
71 
72  bool rm_wrong_shape = world.removeShapeFromObject("ball", box);
73  EXPECT_FALSE(rm_wrong_shape);
74 
75  EXPECT_EQ(2, ball.use_count());
76  EXPECT_EQ(1, box.use_count());
77 
78  // remove ball object
79  bool rm_ball = world.removeShapeFromObject("ball", ball);
80  EXPECT_TRUE(rm_ball);
81 
82  EXPECT_EQ(1, ball.use_count());
83  EXPECT_FALSE(world.hasObject("ball"));
84 
85  // add ball again
86  world.addToObject("ball", ball, Eigen::Isometry3d::Identity());
87 
88  EXPECT_EQ(2, ball.use_count());
89  EXPECT_TRUE(world.hasObject("ball"));
90 
91  EXPECT_FALSE(world.hasObject("mix1"));
92 
93  {
94  std::vector<shapes::ShapeConstPtr> shapes;
95  EigenSTL::vector_Isometry3d poses;
96 
97  shapes.push_back(box);
98  shapes.push_back(cyl);
99  shapes.push_back(ball);
100 
101  poses.push_back(Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)));
102  poses.push_back(Eigen::Isometry3d(Eigen::Translation3d(0, 0, 2)));
103  poses.push_back(Eigen::Isometry3d(Eigen::Translation3d(0, 0, 3)));
104 
105  EXPECT_FALSE(world.hasObject("mix1"));
106 
107  // add mix1 object
108  world.addToObject("mix1", shapes, poses);
109  }
110 
111  EXPECT_TRUE(world.hasObject("mix1"));
112 
113  EXPECT_EQ(2, box.use_count());
114  EXPECT_EQ(2, cyl.use_count());
115  EXPECT_EQ(3, ball.use_count());
116 
117  // add ball2
118  world.addToObject("ball2", ball, Eigen::Isometry3d(Eigen::Translation3d(0, 0, 4)));
119 
120  EXPECT_EQ(2, box.use_count());
121  EXPECT_EQ(2, cyl.use_count());
122  EXPECT_EQ(4, ball.use_count());
123 
124  bool rm_cyl = world.removeShapeFromObject("mix1", cyl);
125  EXPECT_TRUE(rm_cyl);
126 
127  EXPECT_EQ(2, box.use_count());
128  EXPECT_EQ(1, cyl.use_count());
129  EXPECT_EQ(4, ball.use_count());
130 
131  rm_cyl = world.removeShapeFromObject("mix1", cyl);
132  EXPECT_FALSE(rm_cyl);
133 
134  EXPECT_EQ(2, box.use_count());
135  EXPECT_EQ(1, cyl.use_count());
136  EXPECT_EQ(4, ball.use_count());
137 
138  EXPECT_TRUE(world.hasObject("mix1"));
139 
140  EXPECT_EQ(3u, world.size());
141 
142  {
143  World::ObjectConstPtr obj = world.getObject("mix1");
144  EXPECT_EQ(2, obj.use_count());
145 
146  ASSERT_EQ(2u, obj->shapes_.size());
147  ASSERT_EQ(2u, obj->shape_poses_.size());
148 
149  // check translation.z of pose
150  EXPECT_EQ(1.0, obj->shape_poses_[0](2, 3));
151  EXPECT_EQ(3.0, obj->shape_poses_[1](2, 3));
152 
153  move_ok = world.moveShapeInObject("mix1", ball, Eigen::Isometry3d(Eigen::Translation3d(0, 0, 5)));
154  EXPECT_TRUE(move_ok);
155 
156  World::ObjectConstPtr obj2 = world.getObject("mix1");
157  EXPECT_EQ(2, obj2.use_count());
158  EXPECT_EQ(1, obj.use_count());
159 
160  EXPECT_EQ(1.0, obj2->shape_poses_[0](2, 3));
161  EXPECT_EQ(5.0, obj2->shape_poses_[1](2, 3));
162 
163  EXPECT_EQ(1.0, obj->shape_poses_[0](2, 3));
164  EXPECT_EQ(3.0, obj->shape_poses_[1](2, 3));
165 
166  // moving object causes copy, thus extra references to shapes in obj
167  EXPECT_EQ(3, box.use_count());
168  EXPECT_EQ(1, cyl.use_count());
169  EXPECT_EQ(5, ball.use_count());
170 
171  world.removeObject("mix1");
172 
173  EXPECT_EQ(2u, world.size());
174 
175  // no change since obj2 still holds a ref
176  EXPECT_EQ(3, box.use_count());
177  EXPECT_EQ(1, cyl.use_count());
178  EXPECT_EQ(5, ball.use_count());
179 
180  EXPECT_FALSE(world.hasObject("mix1"));
181  EXPECT_TRUE(world.hasObject("ball2"));
182 
183  // ask for nonexistent object
184  World::ObjectConstPtr obj3 = world.getObject("abc");
185  EXPECT_FALSE(obj3);
186  }
187 
188  EXPECT_EQ(1, box.use_count());
189  EXPECT_EQ(1, cyl.use_count());
190  EXPECT_EQ(3, ball.use_count());
191 
192  EXPECT_EQ(2u, world.size());
193 
194  world.clearObjects();
195 
196  EXPECT_EQ(1, box.use_count());
197  EXPECT_EQ(1, cyl.use_count());
198  EXPECT_EQ(1, ball.use_count());
199 
200  EXPECT_FALSE(world.hasObject("mix1"));
201  EXPECT_FALSE(world.hasObject("ball"));
202  EXPECT_FALSE(world.hasObject("ball2"));
203 
204  EXPECT_EQ(0u, world.size());
205 }
206 
207 /* structure to hold copy of callback args */
209 {
212  int cnt_;
213  TestAction() : obj_(""), cnt_(0)
214  {
215  reset();
216  }
217  void reset()
218  {
219  obj_.id_ = "";
220  obj_.shapes_.clear();
221  obj_.shape_poses_.clear();
222  action_ = 0x7f;
223  }
224 };
225 
226 /* notification callback */
227 static void trackChangesNotify(TestAction& ta, const World::ObjectConstPtr& obj, World::Action action)
228 {
229  ta.obj_ = *obj;
230  ta.action_ = action;
231  ta.cnt_++;
232 }
233 
234 TEST(World, TrackChanges)
235 {
236  World world;
237 
238  TestAction ta;
239  World::ObserverHandle observer_ta;
240  observer_ta = world.addObserver([&ta](const World::ObjectConstPtr& object, World::Action action) {
241  return trackChangesNotify(ta, object, action);
242  });
243 
244  // Create some shapes
245  shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
246  shapes::ShapePtr box = std::make_shared<shapes::Box>(1, 2, 3);
247  shapes::ShapePtr cyl = std::make_shared<shapes::Cylinder>(4, 5);
248 
249  world.addToObject("obj1", ball, Eigen::Isometry3d::Identity());
250 
251  EXPECT_EQ(1, ta.cnt_);
252  EXPECT_EQ("obj1", ta.obj_.id_);
253  EXPECT_EQ(World::CREATE | World::ADD_SHAPE, ta.action_);
254  ta.reset();
255 
256  bool move_ok = world.moveShapeInObject("obj1", ball, Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)));
257  EXPECT_TRUE(move_ok);
258 
259  EXPECT_EQ(2, ta.cnt_);
260  EXPECT_EQ("obj1", ta.obj_.id_);
261  EXPECT_EQ(World::MOVE_SHAPE, ta.action_);
262  ta.reset();
263 
264  world.addToObject("obj1", box, Eigen::Isometry3d::Identity());
265 
266  EXPECT_EQ(3, ta.cnt_);
267  EXPECT_EQ("obj1", ta.obj_.id_);
268  EXPECT_EQ(World::ADD_SHAPE, ta.action_);
269  ta.reset();
270 
271  TestAction ta2;
272  World::ObserverHandle observer_ta2;
273  observer_ta2 = world.addObserver([&ta2](const World::ObjectConstPtr& object, World::Action action) {
274  return trackChangesNotify(ta2, object, action);
275  });
276 
277  world.addToObject("obj2", cyl, Eigen::Isometry3d::Identity());
278 
279  EXPECT_EQ(4, ta.cnt_);
280  EXPECT_EQ("obj2", ta.obj_.id_);
281  EXPECT_EQ(World::CREATE | World::ADD_SHAPE, ta.action_);
282  ta.reset();
283  EXPECT_EQ(1, ta2.cnt_);
284  EXPECT_EQ("obj2", ta2.obj_.id_);
285  EXPECT_EQ(World::CREATE | World::ADD_SHAPE, ta2.action_);
286  ta2.reset();
287 
288  world.addToObject("obj3", box, Eigen::Isometry3d::Identity());
289 
290  EXPECT_EQ(5, ta.cnt_);
291  EXPECT_EQ("obj3", ta.obj_.id_);
292  EXPECT_EQ(World::CREATE | World::ADD_SHAPE, ta.action_);
293  ta.reset();
294  EXPECT_EQ(2, ta2.cnt_);
295  EXPECT_EQ("obj3", ta2.obj_.id_);
296  EXPECT_EQ(World::CREATE | World::ADD_SHAPE, ta2.action_);
297  ta2.reset();
298 
299  // remove nonexistent obj
300  bool rm_bad = world.removeShapeFromObject("xyz", ball);
301  EXPECT_FALSE(rm_bad);
302  EXPECT_EQ(5, ta.cnt_);
303  EXPECT_EQ(2, ta2.cnt_);
304 
305  // remove wrong shape
306  rm_bad = world.removeShapeFromObject("obj2", ball);
307  EXPECT_FALSE(rm_bad);
308  EXPECT_EQ(5, ta.cnt_);
309  EXPECT_EQ(2, ta2.cnt_);
310 
311  TestAction ta3;
312  World::ObserverHandle observer_ta3;
313  observer_ta3 = world.addObserver([&ta3](const World::ObjectConstPtr& object, World::Action action) {
314  return trackChangesNotify(ta3, object, action);
315  });
316 
317  bool rm_good = world.removeShapeFromObject("obj2", cyl);
318  EXPECT_TRUE(rm_good);
319 
320  EXPECT_EQ(6, ta.cnt_);
321  EXPECT_EQ("obj2", ta.obj_.id_);
322  EXPECT_EQ(World::DESTROY, ta.action_);
323  ta.reset();
324  EXPECT_EQ(3, ta2.cnt_);
325  EXPECT_EQ("obj2", ta2.obj_.id_);
326  EXPECT_EQ(World::DESTROY, ta2.action_);
327  ta2.reset();
328  EXPECT_EQ(1, ta3.cnt_);
329  EXPECT_EQ("obj2", ta3.obj_.id_);
330  EXPECT_EQ(World::DESTROY, ta3.action_);
331  ta3.reset();
332 
333  world.removeObserver(observer_ta2);
334 
335  rm_good = world.removeShapeFromObject("obj1", ball);
336  EXPECT_TRUE(rm_good);
337 
338  EXPECT_EQ(7, ta.cnt_);
339  EXPECT_EQ("obj1", ta.obj_.id_);
340  EXPECT_EQ(World::REMOVE_SHAPE, ta.action_);
341  ta.reset();
342  EXPECT_EQ(3, ta2.cnt_);
343 
344  EXPECT_EQ(2, ta3.cnt_);
345  EXPECT_EQ("obj1", ta3.obj_.id_);
346  EXPECT_EQ(World::REMOVE_SHAPE, ta3.action_);
347  ta3.reset();
348 
349  // remove all 2 objects (should make 2 DESTROY callbacks per ta)
350  world.clearObjects();
351 
352  EXPECT_EQ(9, ta.cnt_);
353  EXPECT_EQ(World::DESTROY, ta.action_);
354  ta.reset();
355  EXPECT_EQ(3, ta2.cnt_);
356 
357  EXPECT_EQ(4, ta3.cnt_);
358  EXPECT_EQ(World::DESTROY, ta3.action_);
359  ta3.reset();
360 
361  world.removeObserver(observer_ta);
362  world.removeObserver(observer_ta3);
363 
364  EXPECT_EQ(9, ta.cnt_);
365  EXPECT_EQ(3, ta2.cnt_);
366  EXPECT_EQ(4, ta3.cnt_);
367 
368  world.addToObject("obj4", box, Eigen::Isometry3d::Identity());
369 
370  EXPECT_EQ(9, ta.cnt_);
371  EXPECT_EQ(3, ta2.cnt_);
372  EXPECT_EQ(4, ta3.cnt_);
373 }
374 
375 TEST(World, ObjectPoseAndSubframes)
376 {
377  World world;
378 
379  TestAction ta;
380  World::ObserverHandle observer_ta;
381  observer_ta = world.addObserver([&ta](const World::ObjectConstPtr& object, World::Action action) {
382  return trackChangesNotify(ta, object, action);
383  });
384 
385  // Create shapes
386  shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
387  shapes::ShapePtr box = std::make_shared<shapes::Box>(1, 1, 1);
388  shapes::ShapePtr cyl = std::make_shared<shapes::Cylinder>(0.5, 3); // radius, length
389 
390  // Confirm that setting object pose creates an object
391  world.setObjectPose("mix1", Eigen::Isometry3d::Identity());
392 
393  EXPECT_EQ(1, ta.cnt_);
394  EXPECT_EQ("mix1", ta.obj_.id_);
395  EXPECT_EQ(World::CREATE, ta.action_);
396 
397  // Move multi-shape objects, use object pose, use subframes
398  world.addToObject("mix1", box, Eigen::Isometry3d::Identity());
399  world.addToObject("mix1", cyl, Eigen::Isometry3d(Eigen::Translation3d(0, 0, 2)));
400 
402  subframes["frame1"] = Eigen::Isometry3d(Eigen::Translation3d(0, 0, 2));
403  subframes["frame2"] = Eigen::Isometry3d(Eigen::Translation3d(0, 1, 0));
404  world.setSubframesOfObject("mix1", subframes);
405 
406  // Check subframes and shape poses
407  bool found_ok, found_bad;
408  Eigen::Isometry3d pose = world.getTransform("mix1/frame1", found_ok);
409  EXPECT_TRUE(found_ok);
410  EXPECT_EQ(2.0, pose(2, 3)); // check translation.z
411 
412  pose = world.getTransform("mix1/frame2", found_ok);
413  EXPECT_TRUE(found_ok);
414  EXPECT_EQ(1.0, pose(1, 3)); // check translation.y
415  EXPECT_EQ(0.0, pose(2, 3)); // z
416 
417  pose = world.getTransform("mix1/frame3", found_bad);
418  EXPECT_FALSE(found_bad);
419 
420  // Set new object pose, check that all shapes and subframes moved
421  world.setObjectPose("mix1", Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)));
422 
423  pose = world.getTransform("mix1/frame1", found_ok);
424  EXPECT_TRUE(found_ok);
425  EXPECT_EQ(3.0, pose(2, 3)); // z
426 
427  pose = world.getTransform("mix1/frame2", found_ok);
428  EXPECT_TRUE(found_ok);
429  EXPECT_EQ(1.0, pose(1, 3)); // y
430  EXPECT_EQ(1.0, pose(2, 3)); // z
431 
432  pose = world.getGlobalShapeTransform("mix1", 0);
433  EXPECT_TRUE(found_ok);
434  EXPECT_EQ(1.0, pose(2, 3)); // z
435 
436  World::ObjectConstPtr obj = world.getObject("mix1");
437  EXPECT_EQ(0.0, obj->shape_poses_[0](2, 3)); // Internal shape poses do *not* change
438  EXPECT_EQ(2.0, obj->shape_poses_[1](2, 3));
439 
440  // Shift object, check that object pose changed
441  world.moveObject("mix1", Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)));
442 
443  pose = world.getTransform("mix1", found_ok);
444  EXPECT_TRUE(found_ok);
445  EXPECT_EQ(2.0, pose(2, 3)); // z
446 
447  pose = world.getTransform("mix1/frame1", found_ok);
448  EXPECT_TRUE(found_ok);
449  EXPECT_EQ(4.0, pose(2, 3)); // z
450 
451  EXPECT_EQ(0.0, obj->shape_poses_[0](2, 3)); // Internal shape poses should still be constant
452  EXPECT_EQ(2.0, obj->shape_poses_[1](2, 3));
453 
454  // Move object absolute, check object pose
455  world.setObjectPose("mix1", Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)));
456  pose = world.getTransform("mix1", found_ok);
457  EXPECT_TRUE(found_ok);
458  EXPECT_EQ(1.0, pose(2, 3)); // z
459 }
460 
461 int main(int argc, char** argv)
462 {
463  testing::InitGoogleTest(&argc, argv);
464  return RUN_ALL_TESTS();
465 }
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:265
Maintain a representation of the environment.
Definition: world.h:59
const Eigen::Isometry3d & getTransform(const std::string &name) const
Get the transform to an object or subframe with given name. If name does not exist,...
Definition: world.cpp:165
void removeObserver(const ObserverHandle observer_handle)
remove a notifier callback
Definition: world.cpp:389
std::size_t size() const
Definition: world.h:138
bool hasObject(const std::string &object_id) const
Check if a particular object exists in the collision world.
Definition: world.cpp:136
bool removeObject(const std::string &object_id)
Remove a particular object. If there are no external pointers to the corresponding instance of Object...
Definition: world.cpp:326
bool moveShapeInObject(const std::string &object_id, const shapes::ShapeConstPtr &shape, const Eigen::Isometry3d &shape_pose)
Update the pose of a shape in an object. Shape equality is verified by comparing pointers....
Definition: world.cpp:237
bool removeShapeFromObject(const std::string &object_id, const shapes::ShapeConstPtr &shape)
Remove shape from object. Shape equality is verified by comparing pointers. Ownership of the object i...
Definition: world.cpp:295
const Eigen::Isometry3d & getGlobalShapeTransform(const std::string &object_id, int shape_index) const
Get the global transform to a shape of an object with multiple shapes. shape_index is the index of th...
Definition: world.cpp:207
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:77
bool moveObject(const std::string &object_id, const Eigen::Isometry3d &transform)
Move the object pose (thus moving all shapes and subframes in the object) according to the given tran...
Definition: world.cpp:261
ObserverHandle addObserver(const ObserverCallbackFn &callback)
register a callback function for notification of changes. callback will be called right after any cha...
Definition: world.cpp:382
bool setObjectPose(const std::string &object_id, const Eigen::Isometry3d &pose)
Set the pose of an object. The pose is specified in the world frame.
Definition: world.cpp:273
bool setSubframesOfObject(const std::string &object_id, const moveit::core::FixedTransformsMap &subframe_poses)
Set subframes on an object. The frames are relative to the object pose.
Definition: world.cpp:344
void clearObjects()
Clear all objects. If there are no other pointers to corresponding instances of Objects,...
Definition: world.cpp:338
ObjectConstPtr getObject(const std::string &object_id) const
Get a particular object.
Definition: world.cpp:117
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
Definition: transforms.h:53
Definition: world.h:49
void reset()
Definition: test_world.cpp:217
World::Action action_
Definition: test_world.cpp:211
World::Object obj_
Definition: test_world.cpp:210
A representation of an object.
Definition: world.h:79
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string id_
The id for this object.
Definition: world.h:87
TEST(World, AddRemoveShape)
Definition: test_world.cpp:44
int main(int argc, char **argv)
Definition: test_world.cpp:461