00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <gtest/gtest.h>
00038 #include <moveit/collision_detection/world_diff.h>
00039
00040 TEST(WorldDiff, TrackChanges)
00041 {
00042 collision_detection::WorldPtr world(new collision_detection::World);
00043 collision_detection::WorldDiff diff1(world);
00044 collision_detection::WorldDiff diff2;
00045 collision_detection::WorldDiff::const_iterator it;
00046
00047 EXPECT_EQ(0, diff1.getChanges().size());
00048 EXPECT_EQ(0, diff2.getChanges().size());
00049
00050
00051 shapes::ShapePtr ball(new shapes::Sphere(1.0));
00052 shapes::ShapePtr box(new shapes::Box(1,2,3));
00053 shapes::ShapePtr cyl(new shapes::Cylinder(4,5));
00054
00055 world->addToObject("obj1",
00056 ball,
00057 Eigen::Affine3d::Identity());
00058
00059 EXPECT_EQ(1, diff1.getChanges().size());
00060 EXPECT_EQ(0, diff2.getChanges().size());
00061
00062 it = diff1.getChanges().find("obj1");
00063 EXPECT_NE(diff1.end(), it);
00064 EXPECT_EQ(collision_detection::World::CREATE |
00065 collision_detection::World::ADD_SHAPE,
00066 it->second);
00067
00068 it = diff1.getChanges().find("xyz");
00069 EXPECT_EQ(diff1.end(), it);
00070
00071 world->addToObject("obj2",
00072 box,
00073 Eigen::Affine3d::Identity());
00074
00075 EXPECT_EQ(2, diff1.getChanges().size());
00076 EXPECT_EQ(0, diff2.getChanges().size());
00077
00078 it = diff1.getChanges().find("obj2");
00079 EXPECT_NE(diff1.end(), it);
00080 EXPECT_EQ(collision_detection::World::CREATE |
00081 collision_detection::World::ADD_SHAPE,
00082 it->second);
00083
00084 world->addToObject("obj2",
00085 cyl,
00086 Eigen::Affine3d::Identity());
00087
00088 EXPECT_EQ(2, diff1.getChanges().size());
00089 EXPECT_EQ(0, diff2.getChanges().size());
00090
00091 it = diff1.getChanges().find("obj2");
00092 EXPECT_NE(diff1.end(), it);
00093 EXPECT_EQ(collision_detection::World::CREATE |
00094 collision_detection::World::ADD_SHAPE,
00095 it->second);
00096
00097 diff2.reset(world);
00098
00099 bool move_ok = world->moveShapeInObject(
00100 "obj2",
00101 cyl,
00102 Eigen::Affine3d(Eigen::Translation3d(0,0,1)));
00103 EXPECT_TRUE(move_ok);
00104
00105 EXPECT_EQ(2, diff1.getChanges().size());
00106 EXPECT_EQ(1, diff2.getChanges().size());
00107
00108 it = diff1.getChanges().find("obj2");
00109 EXPECT_NE(diff1.end(), it);
00110 EXPECT_EQ(collision_detection::World::CREATE |
00111 collision_detection::World::ADD_SHAPE |
00112 collision_detection::World::MOVE_SHAPE,
00113 it->second);
00114
00115 it = diff2.getChanges().find("obj2");
00116 EXPECT_NE(diff2.end(), it);
00117 EXPECT_EQ(collision_detection::World::MOVE_SHAPE,
00118 it->second);
00119 EXPECT_EQ("obj2", it->first);
00120
00121 diff1.reset(world);
00122
00123 EXPECT_EQ(0, diff1.getChanges().size());
00124 EXPECT_EQ(1, diff2.getChanges().size());
00125
00126 it = diff1.getChanges().find("obj2");
00127 EXPECT_EQ(diff1.end(), it);
00128
00129 world->addToObject("obj3",
00130 box,
00131 Eigen::Affine3d::Identity());
00132
00133 EXPECT_EQ(1, diff1.getChanges().size());
00134 EXPECT_EQ(2, diff2.getChanges().size());
00135
00136 world->addToObject("obj3",
00137 cyl,
00138 Eigen::Affine3d::Identity());
00139
00140 world->addToObject("obj3",
00141 ball,
00142 Eigen::Affine3d::Identity());
00143
00144 EXPECT_EQ(1, diff1.getChanges().size());
00145 EXPECT_EQ(2, diff2.getChanges().size());
00146
00147 diff1.reset();
00148
00149 move_ok = world->moveShapeInObject(
00150 "obj3",
00151 cyl,
00152 Eigen::Affine3d(Eigen::Translation3d(0,0,2)));
00153 EXPECT_TRUE(move_ok);
00154
00155 EXPECT_EQ(0, diff1.getChanges().size());
00156 EXPECT_EQ(2, diff2.getChanges().size());
00157
00158 diff1.reset(world);
00159
00160 world->removeObject("obj2");
00161
00162 EXPECT_EQ(1, diff1.getChanges().size());
00163 EXPECT_EQ(2, diff2.getChanges().size());
00164
00165 it = diff1.getChanges().find("obj2");
00166 EXPECT_NE(diff1.end(), it);
00167 EXPECT_EQ(collision_detection::World::DESTROY,
00168 it->second);
00169 it = diff2.getChanges().find("obj2");
00170 EXPECT_NE(diff2.end(), it);
00171 EXPECT_EQ(collision_detection::World::DESTROY,
00172 it->second);
00173
00174 world->removeShapeFromObject("obj3", cyl);
00175
00176 it = diff1.getChanges().find("obj3");
00177 EXPECT_NE(diff1.end(), it);
00178 EXPECT_EQ(collision_detection::World::REMOVE_SHAPE,
00179 it->second);
00180 it = diff2.getChanges().find("obj3");
00181 EXPECT_NE(diff2.end(), it);
00182 EXPECT_EQ(collision_detection::World::CREATE |
00183 collision_detection::World::ADD_SHAPE |
00184 collision_detection::World::MOVE_SHAPE |
00185 collision_detection::World::REMOVE_SHAPE,
00186 it->second);
00187
00188
00189 world->removeShapeFromObject("obj3", box);
00190
00191 it = diff1.getChanges().find("obj3");
00192 EXPECT_NE(diff1.end(), it);
00193 EXPECT_EQ(collision_detection::World::REMOVE_SHAPE,
00194 it->second);
00195 it = diff2.getChanges().find("obj3");
00196 EXPECT_NE(diff2.end(), it);
00197 EXPECT_EQ(collision_detection::World::CREATE |
00198 collision_detection::World::ADD_SHAPE |
00199 collision_detection::World::MOVE_SHAPE |
00200 collision_detection::World::REMOVE_SHAPE,
00201 it->second);
00202
00203 move_ok = world->moveShapeInObject(
00204 "obj3",
00205 ball,
00206 Eigen::Affine3d(Eigen::Translation3d(0,0,3)));
00207 EXPECT_TRUE(move_ok);
00208
00209 it = diff1.getChanges().find("obj3");
00210 EXPECT_NE(diff1.end(), it);
00211 EXPECT_EQ(collision_detection::World::REMOVE_SHAPE |
00212 collision_detection::World::MOVE_SHAPE,
00213 it->second);
00214 it = diff2.getChanges().find("obj3");
00215 EXPECT_NE(diff2.end(), it);
00216 EXPECT_EQ(collision_detection::World::CREATE |
00217 collision_detection::World::ADD_SHAPE |
00218 collision_detection::World::MOVE_SHAPE |
00219 collision_detection::World::REMOVE_SHAPE,
00220 it->second);
00221
00222 world->removeShapeFromObject("obj3", ball);
00223
00224 it = diff1.getChanges().find("obj3");
00225 EXPECT_NE(diff1.end(), it);
00226 EXPECT_EQ(collision_detection::World::DESTROY,
00227 it->second);
00228 it = diff2.getChanges().find("obj3");
00229 EXPECT_NE(diff2.end(), it);
00230 EXPECT_EQ(collision_detection::World::DESTROY,
00231 it->second);
00232 }
00233
00234 TEST(WorldDiff, SetWorld)
00235 {
00236 collision_detection::WorldPtr world1(new collision_detection::World);
00237 collision_detection::WorldPtr world2(new collision_detection::World);
00238 collision_detection::WorldDiff diff1(world1);
00239 collision_detection::WorldDiff diff1b(world1);
00240 collision_detection::WorldDiff diff2(world2);
00241 collision_detection::WorldDiff::const_iterator it;
00242
00243 shapes::ShapePtr ball(new shapes::Sphere(1.0));
00244 shapes::ShapePtr box(new shapes::Box(1,2,3));
00245 shapes::ShapePtr cyl(new shapes::Cylinder(4,5));
00246
00247 world1->addToObject("objA1",
00248 ball,
00249 Eigen::Affine3d::Identity());
00250
00251 world1->addToObject("objA2",
00252 ball,
00253 Eigen::Affine3d::Identity());
00254
00255 world1->addToObject("objA3",
00256 ball,
00257 Eigen::Affine3d::Identity());
00258
00259 world2->addToObject("objB1",
00260 box,
00261 Eigen::Affine3d::Identity());
00262
00263 world2->addToObject("objB2",
00264 box,
00265 Eigen::Affine3d::Identity());
00266
00267 world2->addToObject("objB3",
00268 box,
00269 Eigen::Affine3d::Identity());
00270
00271 EXPECT_EQ(3, diff1.getChanges().size());
00272 EXPECT_EQ(3, diff1b.getChanges().size());
00273 EXPECT_EQ(3, diff2.getChanges().size());
00274
00275 diff1b.clearChanges();
00276
00277 EXPECT_EQ(3, diff1.getChanges().size());
00278 EXPECT_EQ(0, diff1b.getChanges().size());
00279 EXPECT_EQ(3, diff2.getChanges().size());
00280
00281
00282 diff1.setWorld(world2);
00283
00284 EXPECT_EQ(6, diff1.getChanges().size());
00285 EXPECT_EQ(0, diff1b.getChanges().size());
00286 EXPECT_EQ(3, diff2.getChanges().size());
00287
00288 it = diff1.getChanges().find("objA1");
00289 EXPECT_NE(diff1.end(), it);
00290 EXPECT_EQ(collision_detection::World::DESTROY,
00291 it->second);
00292
00293 it = diff1.getChanges().find("objA2");
00294 EXPECT_NE(diff1.end(), it);
00295 EXPECT_EQ(collision_detection::World::DESTROY,
00296 it->second);
00297
00298 it = diff1.getChanges().find("objA2");
00299 EXPECT_NE(diff1.end(), it);
00300 EXPECT_EQ(collision_detection::World::DESTROY,
00301 it->second);
00302
00303 it = diff1.getChanges().find("objB1");
00304 EXPECT_NE(diff1.end(), it);
00305 EXPECT_EQ(collision_detection::World::CREATE |
00306 collision_detection::World::ADD_SHAPE,
00307 it->second);
00308
00309 it = diff1.getChanges().find("objB2");
00310 EXPECT_NE(diff1.end(), it);
00311 EXPECT_EQ(collision_detection::World::CREATE |
00312 collision_detection::World::ADD_SHAPE,
00313 it->second);
00314
00315 it = diff1.getChanges().find("objB3");
00316 EXPECT_NE(diff1.end(), it);
00317 EXPECT_EQ(collision_detection::World::CREATE |
00318 collision_detection::World::ADD_SHAPE,
00319 it->second);
00320
00321
00322
00323
00324 diff1b.setWorld(world2);
00325
00326 EXPECT_EQ(6, diff1.getChanges().size());
00327 EXPECT_EQ(6, diff1b.getChanges().size());
00328 EXPECT_EQ(3, diff2.getChanges().size());
00329
00330 it = diff1b.getChanges().find("objA1");
00331 EXPECT_NE(diff1b.end(), it);
00332 EXPECT_EQ(collision_detection::World::DESTROY,
00333 it->second);
00334
00335 it = diff1b.getChanges().find("objA2");
00336 EXPECT_NE(diff1b.end(), it);
00337 EXPECT_EQ(collision_detection::World::DESTROY,
00338 it->second);
00339
00340 it = diff1b.getChanges().find("objA2");
00341 EXPECT_NE(diff1b.end(), it);
00342 EXPECT_EQ(collision_detection::World::DESTROY,
00343 it->second);
00344
00345 it = diff1b.getChanges().find("objB1");
00346 EXPECT_NE(diff1b.end(), it);
00347 EXPECT_EQ(collision_detection::World::CREATE |
00348 collision_detection::World::ADD_SHAPE,
00349 it->second);
00350
00351 it = diff1b.getChanges().find("objB2");
00352 EXPECT_NE(diff1b.end(), it);
00353 EXPECT_EQ(collision_detection::World::CREATE |
00354 collision_detection::World::ADD_SHAPE,
00355 it->second);
00356
00357 it = diff1b.getChanges().find("objB3");
00358 EXPECT_NE(diff1b.end(), it);
00359 EXPECT_EQ(collision_detection::World::CREATE |
00360 collision_detection::World::ADD_SHAPE,
00361 it->second);
00362
00363
00364
00365
00366
00367
00368 world1->addToObject("objC",
00369 box,
00370 Eigen::Affine3d::Identity());
00371
00372 EXPECT_EQ(6, diff1.getChanges().size());
00373 EXPECT_EQ(6, diff1b.getChanges().size());
00374 EXPECT_EQ(3, diff2.getChanges().size());
00375
00376
00377 world2->addToObject("objC",
00378 box,
00379 Eigen::Affine3d::Identity());
00380
00381
00382 EXPECT_EQ(7, diff1.getChanges().size());
00383 EXPECT_EQ(7, diff1b.getChanges().size());
00384 EXPECT_EQ(4, diff2.getChanges().size());
00385
00386
00387 diff2.setWorld(world1);
00388
00389 EXPECT_EQ(7, diff1.getChanges().size());
00390 EXPECT_EQ(7, diff1b.getChanges().size());
00391 EXPECT_EQ(7, diff2.getChanges().size());
00392
00393 it = diff2.getChanges().find("objC");
00394 EXPECT_NE(diff2.end(), it);
00395 EXPECT_EQ(collision_detection::World::CREATE |
00396 collision_detection::World::ADD_SHAPE |
00397 collision_detection::World::DESTROY,
00398 it->second);
00399
00400
00401
00402
00403 world1->addToObject("objD",
00404 box,
00405 Eigen::Affine3d::Identity());
00406
00407 EXPECT_EQ(7, diff1.getChanges().size());
00408 EXPECT_EQ(7, diff1b.getChanges().size());
00409 EXPECT_EQ(8, diff2.getChanges().size());
00410
00411 world2->addToObject("objE",
00412 box,
00413 Eigen::Affine3d::Identity());
00414
00415 EXPECT_EQ(8, diff1.getChanges().size());
00416 EXPECT_EQ(8, diff1b.getChanges().size());
00417 EXPECT_EQ(8, diff2.getChanges().size());
00418
00419 }
00420
00421 int main(int argc, char **argv)
00422 {
00423 testing::InitGoogleTest(&argc, argv);
00424 return RUN_ALL_TESTS();
00425 }