moveit2
The MoveIt Motion Planning Framework for ROS 2.
test_world_diff.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>
38 #include <moveit/collision_detection/world_diff.h>
39 #include <geometric_shapes/shapes.h>
40 
41 TEST(WorldDiff, TrackChanges)
42 {
43  collision_detection::WorldPtr world = std::make_shared<collision_detection::World>();
44  collision_detection::WorldDiff diff1(world);
45  collision_detection::WorldDiff diff2;
46  collision_detection::WorldDiff::const_iterator it;
47 
48  EXPECT_EQ(diff1.getChanges().size(), 0u);
49  EXPECT_EQ(diff2.getChanges().size(), 0u);
50 
51  // Create some shapes
52  shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
53  shapes::ShapePtr box = std::make_shared<shapes::Box>(1, 2, 3);
54  shapes::ShapePtr cyl = std::make_shared<shapes::Cylinder>(4, 5);
55 
56  world->addToObject("obj1", ball, Eigen::Isometry3d::Identity());
57 
58  EXPECT_EQ(diff1.getChanges().size(), 1u);
59  EXPECT_EQ(diff2.getChanges().size(), 0u);
60 
61  it = diff1.getChanges().find("obj1");
62  EXPECT_NE(diff1.end(), it);
63  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
64 
65  it = diff1.getChanges().find("xyz");
66  EXPECT_EQ(diff1.end(), it);
67 
68  world->addToObject("obj2", box, Eigen::Isometry3d::Identity());
69 
70  EXPECT_EQ(2u, diff1.getChanges().size());
71  EXPECT_EQ(0u, diff2.getChanges().size());
72 
73  it = diff1.getChanges().find("obj2");
74  EXPECT_NE(diff1.end(), it);
75  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
76 
77  world->addToObject("obj2", cyl, Eigen::Isometry3d::Identity());
78 
79  EXPECT_EQ(2u, diff1.getChanges().size());
80  EXPECT_EQ(0u, diff2.getChanges().size());
81 
82  it = diff1.getChanges().find("obj2");
83  EXPECT_NE(diff1.end(), it);
84  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
85 
86  diff2.reset(world);
87 
88  bool move_ok = world->moveShapeInObject("obj2", cyl, Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1)));
89  EXPECT_TRUE(move_ok);
90 
91  EXPECT_EQ(2u, diff1.getChanges().size());
92  EXPECT_EQ(1u, diff2.getChanges().size());
93 
94  it = diff1.getChanges().find("obj2");
95  EXPECT_NE(diff1.end(), it);
96  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE |
97  collision_detection::World::MOVE_SHAPE,
98  it->second);
99 
100  it = diff2.getChanges().find("obj2");
101  EXPECT_NE(diff2.end(), it);
102  EXPECT_EQ(collision_detection::World::MOVE_SHAPE, it->second);
103  EXPECT_EQ("obj2", it->first);
104 
105  diff1.reset(world);
106 
107  EXPECT_EQ(0u, diff1.getChanges().size());
108  EXPECT_EQ(1u, diff2.getChanges().size());
109 
110  it = diff1.getChanges().find("obj2");
111  EXPECT_EQ(diff1.end(), it);
112 
113  world->addToObject("obj3", box, Eigen::Isometry3d::Identity());
114 
115  EXPECT_EQ(1u, diff1.getChanges().size());
116  EXPECT_EQ(2u, diff2.getChanges().size());
117 
118  world->addToObject("obj3", cyl, Eigen::Isometry3d::Identity());
119 
120  world->addToObject("obj3", ball, Eigen::Isometry3d::Identity());
121 
122  EXPECT_EQ(1u, diff1.getChanges().size());
123  EXPECT_EQ(2u, diff2.getChanges().size());
124 
125  diff1.reset();
126 
127  move_ok = world->moveShapeInObject("obj3", cyl, Eigen::Isometry3d(Eigen::Translation3d(0, 0, 2)));
128  EXPECT_TRUE(move_ok);
129 
130  EXPECT_EQ(0u, diff1.getChanges().size());
131  EXPECT_EQ(2u, diff2.getChanges().size());
132 
133  diff1.reset(world);
134 
135  world->removeObject("obj2");
136 
137  EXPECT_EQ(1u, diff1.getChanges().size());
138  EXPECT_EQ(2u, diff2.getChanges().size());
139 
140  it = diff1.getChanges().find("obj2");
141  EXPECT_NE(diff1.end(), it);
142  EXPECT_EQ(collision_detection::World::DESTROY, it->second);
143  it = diff2.getChanges().find("obj2");
144  EXPECT_NE(diff2.end(), it);
145  EXPECT_EQ(collision_detection::World::DESTROY, it->second);
146 
147  world->removeShapeFromObject("obj3", cyl);
148 
149  it = diff1.getChanges().find("obj3");
150  EXPECT_NE(diff1.end(), it);
151  EXPECT_EQ(collision_detection::World::REMOVE_SHAPE, it->second);
152  it = diff2.getChanges().find("obj3");
153  EXPECT_NE(diff2.end(), it);
154  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE |
155  collision_detection::World::MOVE_SHAPE | collision_detection::World::REMOVE_SHAPE,
156  it->second);
157 
158  world->removeShapeFromObject("obj3", box);
159 
160  it = diff1.getChanges().find("obj3");
161  EXPECT_NE(diff1.end(), it);
162  EXPECT_EQ(collision_detection::World::REMOVE_SHAPE, it->second);
163  it = diff2.getChanges().find("obj3");
164  EXPECT_NE(diff2.end(), it);
165  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE |
166  collision_detection::World::MOVE_SHAPE | collision_detection::World::REMOVE_SHAPE,
167  it->second);
168 
169  move_ok = world->moveShapeInObject("obj3", ball, Eigen::Isometry3d(Eigen::Translation3d(0, 0, 3)));
170  EXPECT_TRUE(move_ok);
171 
172  it = diff1.getChanges().find("obj3");
173  EXPECT_NE(diff1.end(), it);
174  EXPECT_EQ(collision_detection::World::REMOVE_SHAPE | collision_detection::World::MOVE_SHAPE, it->second);
175  it = diff2.getChanges().find("obj3");
176  EXPECT_NE(diff2.end(), it);
177  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE |
178  collision_detection::World::MOVE_SHAPE | collision_detection::World::REMOVE_SHAPE,
179  it->second);
180 
181  world->removeShapeFromObject("obj3", ball);
182 
183  it = diff1.getChanges().find("obj3");
184  EXPECT_NE(diff1.end(), it);
185  EXPECT_EQ(collision_detection::World::DESTROY, it->second);
186  it = diff2.getChanges().find("obj3");
187  EXPECT_NE(diff2.end(), it);
188  EXPECT_EQ(collision_detection::World::DESTROY, it->second);
189 }
190 
191 TEST(WorldDiff, SetWorld)
192 {
193  collision_detection::WorldPtr world1 = std::make_shared<collision_detection::World>();
194  collision_detection::WorldPtr world2 = std::make_shared<collision_detection::World>();
195  collision_detection::WorldDiff diff1(world1);
196  collision_detection::WorldDiff diff1b(world1);
197  collision_detection::WorldDiff diff2(world2);
198  collision_detection::WorldDiff::const_iterator it;
199 
200  shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
201  shapes::ShapePtr box = std::make_shared<shapes::Box>(1, 2, 3);
202  shapes::ShapePtr cyl = std::make_shared<shapes::Cylinder>(4, 5);
203 
204  world1->addToObject("objA1", ball, Eigen::Isometry3d::Identity());
205 
206  world1->addToObject("objA2", ball, Eigen::Isometry3d::Identity());
207 
208  world1->addToObject("objA3", ball, Eigen::Isometry3d::Identity());
209 
210  world2->addToObject("objB1", box, Eigen::Isometry3d::Identity());
211 
212  world2->addToObject("objB2", box, Eigen::Isometry3d::Identity());
213 
214  world2->addToObject("objB3", box, Eigen::Isometry3d::Identity());
215 
216  EXPECT_EQ(3u, diff1.getChanges().size());
217  EXPECT_EQ(3u, diff1b.getChanges().size());
218  EXPECT_EQ(3u, diff2.getChanges().size());
219 
220  diff1b.clearChanges();
221 
222  EXPECT_EQ(3u, diff1.getChanges().size());
223  EXPECT_EQ(0u, diff1b.getChanges().size());
224  EXPECT_EQ(3u, diff2.getChanges().size());
225 
226  diff1.setWorld(world2);
227 
228  EXPECT_EQ(6u, diff1.getChanges().size());
229  EXPECT_EQ(0u, diff1b.getChanges().size());
230  EXPECT_EQ(3u, diff2.getChanges().size());
231 
232  it = diff1.getChanges().find("objA1");
233  EXPECT_NE(diff1.end(), it);
234  EXPECT_EQ(collision_detection::World::DESTROY, it->second);
235 
236  it = diff1.getChanges().find("objA2");
237  EXPECT_NE(diff1.end(), it);
238  EXPECT_EQ(collision_detection::World::DESTROY, it->second);
239 
240  it = diff1.getChanges().find("objA2");
241  EXPECT_NE(diff1.end(), it);
242  EXPECT_EQ(collision_detection::World::DESTROY, it->second);
243 
244  it = diff1.getChanges().find("objB1");
245  EXPECT_NE(diff1.end(), it);
246  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
247 
248  it = diff1.getChanges().find("objB2");
249  EXPECT_NE(diff1.end(), it);
250  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
251 
252  it = diff1.getChanges().find("objB3");
253  EXPECT_NE(diff1.end(), it);
254  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
255 
256  diff1b.setWorld(world2);
257 
258  EXPECT_EQ(6u, diff1.getChanges().size());
259  EXPECT_EQ(6u, diff1b.getChanges().size());
260  EXPECT_EQ(3u, diff2.getChanges().size());
261 
262  it = diff1b.getChanges().find("objA1");
263  EXPECT_NE(diff1b.end(), it);
264  EXPECT_EQ(collision_detection::World::DESTROY, it->second);
265 
266  it = diff1b.getChanges().find("objA2");
267  EXPECT_NE(diff1b.end(), it);
268  EXPECT_EQ(collision_detection::World::DESTROY, it->second);
269 
270  it = diff1b.getChanges().find("objA2");
271  EXPECT_NE(diff1b.end(), it);
272  EXPECT_EQ(collision_detection::World::DESTROY, it->second);
273 
274  it = diff1b.getChanges().find("objB1");
275  EXPECT_NE(diff1b.end(), it);
276  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
277 
278  it = diff1b.getChanges().find("objB2");
279  EXPECT_NE(diff1b.end(), it);
280  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
281 
282  it = diff1b.getChanges().find("objB3");
283  EXPECT_NE(diff1b.end(), it);
284  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
285 
286  world1->addToObject("objC", box, Eigen::Isometry3d::Identity());
287 
288  EXPECT_EQ(6u, diff1.getChanges().size());
289  EXPECT_EQ(6u, diff1b.getChanges().size());
290  EXPECT_EQ(3u, diff2.getChanges().size());
291 
292  world2->addToObject("objC", box, Eigen::Isometry3d::Identity());
293 
294  EXPECT_EQ(7u, diff1.getChanges().size());
295  EXPECT_EQ(7u, diff1b.getChanges().size());
296  EXPECT_EQ(4u, diff2.getChanges().size());
297 
298  diff2.setWorld(world1);
299 
300  EXPECT_EQ(7u, diff1.getChanges().size());
301  EXPECT_EQ(7u, diff1b.getChanges().size());
302  EXPECT_EQ(7u, diff2.getChanges().size());
303 
304  it = diff2.getChanges().find("objC");
305  EXPECT_NE(diff2.end(), it);
306  EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE |
307  collision_detection::World::DESTROY,
308  it->second);
309 
310  world1->addToObject("objD", box, Eigen::Isometry3d::Identity());
311 
312  EXPECT_EQ(7u, diff1.getChanges().size());
313  EXPECT_EQ(7u, diff1b.getChanges().size());
314  EXPECT_EQ(8u, diff2.getChanges().size());
315 
316  world2->addToObject("objE", box, Eigen::Isometry3d::Identity());
317 
318  EXPECT_EQ(8u, diff1.getChanges().size());
319  EXPECT_EQ(8u, diff1b.getChanges().size());
320  EXPECT_EQ(8u, diff2.getChanges().size());
321 }
322 
323 int main(int argc, char** argv)
324 {
325  testing::InitGoogleTest(&argc, argv);
326  return RUN_ALL_TESTS();
327 }
collision_detection::WorldDiff
Maintain a diff list of changes that have happened to a World.
Definition: world_diff.h:48
collision_detection::WorldDiff::getChanges
const std::map< std::string, World::Action > & getChanges() const
Return all the changes that have been recorded.
Definition: world_diff.h:74
collision_detection::WorldDiff::reset
void reset(const WorldPtr &world)
Set which world to record. Erases all previously recorded changes.
Definition: world_diff.cpp:83
collision_detection::WorldDiff::setWorld
void setWorld(const WorldPtr &world)
Set which world to record. Records all objects in old world (if any) as DESTROYED and all objects in ...
Definition: world_diff.cpp:96
collision_detection::WorldDiff::clearChanges
void clearChanges()
Clear the internally maintained vector of changes.
Definition: world_diff.cpp:112
collision_detection::WorldDiff::end
const_iterator end() const
Definition: world_diff.h:86
collision_detection::WorldDiff::const_iterator
std::map< std::string, World::Action >::const_iterator const_iterator
Definition: world_diff.h:79
collision_detection::World::ADD_SHAPE
@ ADD_SHAPE
Definition: world.h:257
collision_detection::World::DESTROY
@ DESTROY
Definition: world.h:255
collision_detection::World::CREATE
@ CREATE
Definition: world.h:254
collision_detection::World::REMOVE_SHAPE
@ REMOVE_SHAPE
Definition: world.h:258
collision_detection::World::MOVE_SHAPE
@ MOVE_SHAPE
Definition: world.h:256
TEST
TEST(WorldDiff, TrackChanges)
Definition: test_world_diff.cpp:41
main
int main(int argc, char **argv)
Definition: test_world_diff.cpp:323
world_diff.h
Generated by doxygen 1.9.1