moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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
41TEST(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
191TEST(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
323int 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::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::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::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:260
collision_detection::World::DESTROY
@ DESTROY
Definition world.h:258
collision_detection::World::CREATE
@ CREATE
Definition world.h:257
collision_detection::World::REMOVE_SHAPE
@ REMOVE_SHAPE
Definition world.h:261
collision_detection::World::MOVE_SHAPE
@ MOVE_SHAPE
Definition world.h:259
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.8