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>
40 #include <boost/bind.hpp>
41 
42 TEST(World, AddRemoveShape)
43 {
45 
46  // Create some shapes
47  shapes::ShapePtr ball(new shapes::Sphere(1.0));
48  shapes::ShapePtr box(new shapes::Box(1, 2, 3));
49  shapes::ShapePtr cyl(new shapes::Cylinder(4, 5));
50 
51  EXPECT_EQ(1, ball.use_count());
52 
53  EXPECT_FALSE(world.hasObject("ball"));
54 
55  // Add ball object
56  world.addToObject("ball", ball, Eigen::Affine3d::Identity());
57 
58  EXPECT_EQ(2, ball.use_count());
59  EXPECT_TRUE(world.hasObject("ball"));
60 
61  bool move_ok = world.moveShapeInObject("ball", ball, Eigen::Affine3d(Eigen::Translation3d(0, 0, 9)));
62  EXPECT_TRUE(move_ok);
63 
64  EXPECT_EQ(2, ball.use_count());
65  EXPECT_TRUE(world.hasObject("ball"));
66 
67  bool rm_nonexistant = world.removeShapeFromObject("xyz", ball);
68  EXPECT_FALSE(rm_nonexistant);
69 
70  bool rm_wrong_shape = world.removeShapeFromObject("ball", box);
71  EXPECT_FALSE(rm_wrong_shape);
72 
73  EXPECT_EQ(2, ball.use_count());
74  EXPECT_EQ(1, box.use_count());
75 
76  // remove ball object
77  bool rm_ball = world.removeShapeFromObject("ball", ball);
78  EXPECT_TRUE(rm_ball);
79 
80  EXPECT_EQ(1, ball.use_count());
81  EXPECT_FALSE(world.hasObject("ball"));
82 
83  // add ball again
84  world.addToObject("ball", ball, Eigen::Affine3d::Identity());
85 
86  EXPECT_EQ(2, ball.use_count());
87  EXPECT_TRUE(world.hasObject("ball"));
88 
89  EXPECT_FALSE(world.hasObject("mix1"));
90 
91  {
92  std::vector<shapes::ShapeConstPtr> shapes;
94 
95  shapes.push_back(box);
96  shapes.push_back(cyl);
97  shapes.push_back(ball);
98 
99  poses.push_back(Eigen::Affine3d(Eigen::Translation3d(0, 0, 1)));
100  poses.push_back(Eigen::Affine3d(Eigen::Translation3d(0, 0, 2)));
101  poses.push_back(Eigen::Affine3d(Eigen::Translation3d(0, 0, 3)));
102 
103  EXPECT_FALSE(world.hasObject("mix1"));
104 
105  // add mix1 object
106  world.addToObject("mix1", shapes, poses);
107  }
108 
109  EXPECT_TRUE(world.hasObject("mix1"));
110 
111  EXPECT_EQ(2, box.use_count());
112  EXPECT_EQ(2, cyl.use_count());
113  EXPECT_EQ(3, ball.use_count());
114 
115  // add ball2
116  world.addToObject("ball2", ball, Eigen::Affine3d(Eigen::Translation3d(0, 0, 4)));
117 
118  EXPECT_EQ(2, box.use_count());
119  EXPECT_EQ(2, cyl.use_count());
120  EXPECT_EQ(4, ball.use_count());
121 
122  bool rm_cyl = world.removeShapeFromObject("mix1", cyl);
123  EXPECT_TRUE(rm_cyl);
124 
125  EXPECT_EQ(2, box.use_count());
126  EXPECT_EQ(1, cyl.use_count());
127  EXPECT_EQ(4, ball.use_count());
128 
129  rm_cyl = world.removeShapeFromObject("mix1", cyl);
130  EXPECT_FALSE(rm_cyl);
131 
132  EXPECT_EQ(2, box.use_count());
133  EXPECT_EQ(1, cyl.use_count());
134  EXPECT_EQ(4, ball.use_count());
135 
136  EXPECT_TRUE(world.hasObject("mix1"));
137 
138  EXPECT_EQ(3, world.size());
139 
140  {
141  collision_detection::World::ObjectConstPtr obj = world.getObject("mix1");
142  EXPECT_EQ(2, obj.use_count());
143 
144  ASSERT_EQ(2, obj->shapes_.size());
145  ASSERT_EQ(2, obj->shape_poses_.size());
146 
147  // check translation.z of pose
148  EXPECT_EQ(1.0, obj->shape_poses_[0](2, 3));
149  EXPECT_EQ(3.0, obj->shape_poses_[1](2, 3));
150 
151  move_ok = world.moveShapeInObject("mix1", ball, Eigen::Affine3d(Eigen::Translation3d(0, 0, 5)));
152  EXPECT_TRUE(move_ok);
153 
154  collision_detection::World::ObjectConstPtr obj2 = world.getObject("mix1");
155  EXPECT_EQ(2, obj2.use_count());
156  EXPECT_EQ(1, obj.use_count());
157 
158  EXPECT_EQ(1.0, obj2->shape_poses_[0](2, 3));
159  EXPECT_EQ(5.0, obj2->shape_poses_[1](2, 3));
160 
161  EXPECT_EQ(1.0, obj->shape_poses_[0](2, 3));
162  EXPECT_EQ(3.0, obj->shape_poses_[1](2, 3));
163 
164  // moving object causes copy, thus extra references to shapes in obj
165  EXPECT_EQ(3, box.use_count());
166  EXPECT_EQ(1, cyl.use_count());
167  EXPECT_EQ(5, ball.use_count());
168 
169  world.removeObject("mix1");
170 
171  EXPECT_EQ(2, world.size());
172 
173  // no change since obj2 still holds a ref
174  EXPECT_EQ(3, box.use_count());
175  EXPECT_EQ(1, cyl.use_count());
176  EXPECT_EQ(5, ball.use_count());
177 
178  EXPECT_FALSE(world.hasObject("mix1"));
179  EXPECT_TRUE(world.hasObject("ball2"));
180 
181  // ask for nonexistant object
182  collision_detection::World::ObjectConstPtr obj3 = world.getObject("abc");
183  EXPECT_FALSE(obj3);
184  }
185 
186  EXPECT_EQ(1, box.use_count());
187  EXPECT_EQ(1, cyl.use_count());
188  EXPECT_EQ(3, ball.use_count());
189 
190  EXPECT_EQ(2, world.size());
191 
192  world.clearObjects();
193 
194  EXPECT_EQ(1, box.use_count());
195  EXPECT_EQ(1, cyl.use_count());
196  EXPECT_EQ(1, ball.use_count());
197 
198  EXPECT_FALSE(world.hasObject("mix1"));
199  EXPECT_FALSE(world.hasObject("ball"));
200  EXPECT_FALSE(world.hasObject("ball2"));
201 
202  EXPECT_EQ(0, world.size());
203 }
204 
205 /* structure to hold copy of callback args */
207 {
210  int cnt_;
211  TestAction() : obj_(""), cnt_(0)
212  {
213  reset();
214  }
215  void reset()
216  {
217  obj_.id_ = "";
218  obj_.shapes_.clear();
219  obj_.shape_poses_.clear();
220  action_ = 0x7f;
221  }
222 };
223 
224 /* notification callback */
225 static void TrackChangesNotify(TestAction* ta, const collision_detection::World::ObjectConstPtr& obj,
227 {
228  ta->obj_ = *obj;
229  ta->action_ = action;
230  ta->cnt_++;
231 }
232 
233 TEST(World, TrackChanges)
234 {
236 
237  TestAction ta;
239  observer_ta = world.addObserver(boost::bind(TrackChangesNotify, &ta, _1, _2));
240 
241  // Create some shapes
242  shapes::ShapePtr ball(new shapes::Sphere(1.0));
243  shapes::ShapePtr box(new shapes::Box(1, 2, 3));
244  shapes::ShapePtr cyl(new shapes::Cylinder(4, 5));
245 
246  world.addToObject("obj1", ball, Eigen::Affine3d::Identity());
247 
248  EXPECT_EQ(1, ta.cnt_);
249  EXPECT_EQ("obj1", ta.obj_.id_);
251  ta.reset();
252 
253  bool move_ok = world.moveShapeInObject("obj1", ball, Eigen::Affine3d(Eigen::Translation3d(0, 0, 1)));
254  EXPECT_TRUE(move_ok);
255 
256  EXPECT_EQ(2, ta.cnt_);
257  EXPECT_EQ("obj1", ta.obj_.id_);
259  ta.reset();
260 
261  world.addToObject("obj1", box, Eigen::Affine3d::Identity());
262 
263  EXPECT_EQ(3, ta.cnt_);
264  EXPECT_EQ("obj1", ta.obj_.id_);
266  ta.reset();
267 
268  TestAction ta2;
270  observer_ta2 = world.addObserver(boost::bind(TrackChangesNotify, &ta2, _1, _2));
271 
272  world.addToObject("obj2", cyl, Eigen::Affine3d::Identity());
273 
274  EXPECT_EQ(4, ta.cnt_);
275  EXPECT_EQ("obj2", ta.obj_.id_);
277  ta.reset();
278  EXPECT_EQ(1, ta2.cnt_);
279  EXPECT_EQ("obj2", ta2.obj_.id_);
281  ta2.reset();
282 
283  world.addToObject("obj3", box, Eigen::Affine3d::Identity());
284 
285  EXPECT_EQ(5, ta.cnt_);
286  EXPECT_EQ("obj3", ta.obj_.id_);
288  ta.reset();
289  EXPECT_EQ(2, ta2.cnt_);
290  EXPECT_EQ("obj3", ta2.obj_.id_);
292  ta2.reset();
293 
294  // remove nonexistent obj
295  bool rm_bad = world.removeShapeFromObject("xyz", ball);
296  EXPECT_FALSE(rm_bad);
297  EXPECT_EQ(5, ta.cnt_);
298  EXPECT_EQ(2, ta2.cnt_);
299 
300  // remove wrong shape
301  rm_bad = world.removeShapeFromObject("obj2", ball);
302  EXPECT_FALSE(rm_bad);
303  EXPECT_EQ(5, ta.cnt_);
304  EXPECT_EQ(2, ta2.cnt_);
305 
306  TestAction ta3;
308  observer_ta3 = world.addObserver(boost::bind(TrackChangesNotify, &ta3, _1, _2));
309 
310  bool rm_good = world.removeShapeFromObject("obj2", cyl);
311  EXPECT_TRUE(rm_good);
312 
313  EXPECT_EQ(6, ta.cnt_);
314  EXPECT_EQ("obj2", ta.obj_.id_);
316  ta.reset();
317  EXPECT_EQ(3, ta2.cnt_);
318  EXPECT_EQ("obj2", ta2.obj_.id_);
320  ta2.reset();
321  EXPECT_EQ(1, ta3.cnt_);
322  EXPECT_EQ("obj2", ta3.obj_.id_);
324  ta3.reset();
325 
326  world.removeObserver(observer_ta2);
327 
328  rm_good = world.removeShapeFromObject("obj1", ball);
329  EXPECT_TRUE(rm_good);
330 
331  EXPECT_EQ(7, ta.cnt_);
332  EXPECT_EQ("obj1", ta.obj_.id_);
334  ta.reset();
335  EXPECT_EQ(3, ta2.cnt_);
336 
337  EXPECT_EQ(2, ta3.cnt_);
338  EXPECT_EQ("obj1", ta3.obj_.id_);
340  ta3.reset();
341 
342  // remove all 2 objects (should make 2 DESTROY callbacks per ta)
343  world.clearObjects();
344 
345  EXPECT_EQ(9, ta.cnt_);
347  ta.reset();
348  EXPECT_EQ(3, ta2.cnt_);
349 
350  EXPECT_EQ(4, ta3.cnt_);
352  ta3.reset();
353 
354  world.removeObserver(observer_ta);
355  world.removeObserver(observer_ta3);
356 
357  EXPECT_EQ(9, ta.cnt_);
358  EXPECT_EQ(3, ta2.cnt_);
359  EXPECT_EQ(4, ta3.cnt_);
360 
361  world.addToObject("obj4", box, Eigen::Affine3d::Identity());
362 
363  EXPECT_EQ(9, ta.cnt_);
364  EXPECT_EQ(3, ta2.cnt_);
365  EXPECT_EQ(4, ta3.cnt_);
366 }
367 
368 int main(int argc, char** argv)
369 {
370  testing::InitGoogleTest(&argc, argv);
371  return RUN_ALL_TESTS();
372 }
bool removeObject(const std::string &id)
Remove a particular object. If there are no external pointers to the corresponding instance of Object...
Definition: world.cpp:202
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void clearObjects()
Clear all objects. If there are no other pointers to corresponding instances of Objects, the memory is freed.
Definition: world.cpp:214
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
Definition: world.h:102
ObserverHandle addObserver(const ObserverCallbackFn &callback)
register a callback function for notification of changes. callback will be called right after any cha...
Definition: world.cpp:220
Maintain a representation of the environment.
Definition: world.h:60
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:195
void addToObject(const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &poses)
Add shapes to an object in the map. This function makes repeated calls to addToObjectInternal() to ad...
Definition: world.cpp:64
std::shared_ptr< Shape > ShapePtr
bool hasObject(const std::string &id) const
Check if a particular object exists in the collision world.
Definition: world.cpp:134
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string id_
The id for this object.
Definition: world.h:89
#define EXPECT_FALSE(args)
#define EXPECT_EQ(a, b)
A representation of an object.
Definition: world.h:80
std::size_t size() const
Definition: world.h:129
TEST(World, AddRemoveShape)
Definition: test_world.cpp:42
void removeObserver(const ObserverHandle observer_handle)
remove a notifier callback
Definition: world.cpp:227
bool removeShapeFromObject(const std::string &id, const shapes::ShapeConstPtr &shape)
Remove shape from object. Shape equality is verified by comparing pointers. Ownership of the object i...
Definition: world.cpp:174
void reset()
Definition: test_world.cpp:215
collision_detection::World::Action action_
Definition: test_world.cpp:209
ObjectConstPtr getObject(const std::string &id) const
Get a particular object.
Definition: world.cpp:119
bool moveShapeInObject(const std::string &id, const shapes::ShapeConstPtr &shape, const Eigen::Affine3d &pose)
Update the pose of a shape in an object. Shape equality is verified by comparing pointers. Returns true on success.
Definition: world.cpp:139
EigenSTL::vector_Affine3d shape_poses_
The poses of the corresponding entries in shapes_.
Definition: world.h:107
int main(int argc, char **argv)
Definition: test_world.cpp:368
#define EXPECT_TRUE(args)
static void TrackChangesNotify(TestAction *ta, const collision_detection::World::ObjectConstPtr &obj, collision_detection::World::Action action)
Definition: test_world.cpp:225
collision_detection::World::Object obj_
Definition: test_world.cpp:208


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33