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", ball, Eigen::Affine3d::Identity());
00056
00057 EXPECT_EQ(1, diff1.getChanges().size());
00058 EXPECT_EQ(0, diff2.getChanges().size());
00059
00060 it = diff1.getChanges().find("obj1");
00061 EXPECT_NE(diff1.end(), it);
00062 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
00063
00064 it = diff1.getChanges().find("xyz");
00065 EXPECT_EQ(diff1.end(), it);
00066
00067 world->addToObject("obj2", box, Eigen::Affine3d::Identity());
00068
00069 EXPECT_EQ(2, diff1.getChanges().size());
00070 EXPECT_EQ(0, diff2.getChanges().size());
00071
00072 it = diff1.getChanges().find("obj2");
00073 EXPECT_NE(diff1.end(), it);
00074 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
00075
00076 world->addToObject("obj2", cyl, Eigen::Affine3d::Identity());
00077
00078 EXPECT_EQ(2, diff1.getChanges().size());
00079 EXPECT_EQ(0, diff2.getChanges().size());
00080
00081 it = diff1.getChanges().find("obj2");
00082 EXPECT_NE(diff1.end(), it);
00083 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
00084
00085 diff2.reset(world);
00086
00087 bool move_ok = world->moveShapeInObject("obj2", cyl, Eigen::Affine3d(Eigen::Translation3d(0, 0, 1)));
00088 EXPECT_TRUE(move_ok);
00089
00090 EXPECT_EQ(2, diff1.getChanges().size());
00091 EXPECT_EQ(1, diff2.getChanges().size());
00092
00093 it = diff1.getChanges().find("obj2");
00094 EXPECT_NE(diff1.end(), it);
00095 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE |
00096 collision_detection::World::MOVE_SHAPE,
00097 it->second);
00098
00099 it = diff2.getChanges().find("obj2");
00100 EXPECT_NE(diff2.end(), it);
00101 EXPECT_EQ(collision_detection::World::MOVE_SHAPE, it->second);
00102 EXPECT_EQ("obj2", it->first);
00103
00104 diff1.reset(world);
00105
00106 EXPECT_EQ(0, diff1.getChanges().size());
00107 EXPECT_EQ(1, diff2.getChanges().size());
00108
00109 it = diff1.getChanges().find("obj2");
00110 EXPECT_EQ(diff1.end(), it);
00111
00112 world->addToObject("obj3", box, Eigen::Affine3d::Identity());
00113
00114 EXPECT_EQ(1, diff1.getChanges().size());
00115 EXPECT_EQ(2, diff2.getChanges().size());
00116
00117 world->addToObject("obj3", cyl, Eigen::Affine3d::Identity());
00118
00119 world->addToObject("obj3", ball, Eigen::Affine3d::Identity());
00120
00121 EXPECT_EQ(1, diff1.getChanges().size());
00122 EXPECT_EQ(2, diff2.getChanges().size());
00123
00124 diff1.reset();
00125
00126 move_ok = world->moveShapeInObject("obj3", cyl, Eigen::Affine3d(Eigen::Translation3d(0, 0, 2)));
00127 EXPECT_TRUE(move_ok);
00128
00129 EXPECT_EQ(0, diff1.getChanges().size());
00130 EXPECT_EQ(2, diff2.getChanges().size());
00131
00132 diff1.reset(world);
00133
00134 world->removeObject("obj2");
00135
00136 EXPECT_EQ(1, diff1.getChanges().size());
00137 EXPECT_EQ(2, diff2.getChanges().size());
00138
00139 it = diff1.getChanges().find("obj2");
00140 EXPECT_NE(diff1.end(), it);
00141 EXPECT_EQ(collision_detection::World::DESTROY, it->second);
00142 it = diff2.getChanges().find("obj2");
00143 EXPECT_NE(diff2.end(), it);
00144 EXPECT_EQ(collision_detection::World::DESTROY, it->second);
00145
00146 world->removeShapeFromObject("obj3", cyl);
00147
00148 it = diff1.getChanges().find("obj3");
00149 EXPECT_NE(diff1.end(), it);
00150 EXPECT_EQ(collision_detection::World::REMOVE_SHAPE, it->second);
00151 it = diff2.getChanges().find("obj3");
00152 EXPECT_NE(diff2.end(), it);
00153 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE |
00154 collision_detection::World::MOVE_SHAPE | collision_detection::World::REMOVE_SHAPE,
00155 it->second);
00156
00157 world->removeShapeFromObject("obj3", box);
00158
00159 it = diff1.getChanges().find("obj3");
00160 EXPECT_NE(diff1.end(), it);
00161 EXPECT_EQ(collision_detection::World::REMOVE_SHAPE, it->second);
00162 it = diff2.getChanges().find("obj3");
00163 EXPECT_NE(diff2.end(), it);
00164 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE |
00165 collision_detection::World::MOVE_SHAPE | collision_detection::World::REMOVE_SHAPE,
00166 it->second);
00167
00168 move_ok = world->moveShapeInObject("obj3", ball, Eigen::Affine3d(Eigen::Translation3d(0, 0, 3)));
00169 EXPECT_TRUE(move_ok);
00170
00171 it = diff1.getChanges().find("obj3");
00172 EXPECT_NE(diff1.end(), it);
00173 EXPECT_EQ(collision_detection::World::REMOVE_SHAPE | collision_detection::World::MOVE_SHAPE, it->second);
00174 it = diff2.getChanges().find("obj3");
00175 EXPECT_NE(diff2.end(), it);
00176 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE |
00177 collision_detection::World::MOVE_SHAPE | collision_detection::World::REMOVE_SHAPE,
00178 it->second);
00179
00180 world->removeShapeFromObject("obj3", ball);
00181
00182 it = diff1.getChanges().find("obj3");
00183 EXPECT_NE(diff1.end(), it);
00184 EXPECT_EQ(collision_detection::World::DESTROY, it->second);
00185 it = diff2.getChanges().find("obj3");
00186 EXPECT_NE(diff2.end(), it);
00187 EXPECT_EQ(collision_detection::World::DESTROY, it->second);
00188 }
00189
00190 TEST(WorldDiff, SetWorld)
00191 {
00192 collision_detection::WorldPtr world1(new collision_detection::World);
00193 collision_detection::WorldPtr world2(new collision_detection::World);
00194 collision_detection::WorldDiff diff1(world1);
00195 collision_detection::WorldDiff diff1b(world1);
00196 collision_detection::WorldDiff diff2(world2);
00197 collision_detection::WorldDiff::const_iterator it;
00198
00199 shapes::ShapePtr ball(new shapes::Sphere(1.0));
00200 shapes::ShapePtr box(new shapes::Box(1, 2, 3));
00201 shapes::ShapePtr cyl(new shapes::Cylinder(4, 5));
00202
00203 world1->addToObject("objA1", ball, Eigen::Affine3d::Identity());
00204
00205 world1->addToObject("objA2", ball, Eigen::Affine3d::Identity());
00206
00207 world1->addToObject("objA3", ball, Eigen::Affine3d::Identity());
00208
00209 world2->addToObject("objB1", box, Eigen::Affine3d::Identity());
00210
00211 world2->addToObject("objB2", box, Eigen::Affine3d::Identity());
00212
00213 world2->addToObject("objB3", box, Eigen::Affine3d::Identity());
00214
00215 EXPECT_EQ(3, diff1.getChanges().size());
00216 EXPECT_EQ(3, diff1b.getChanges().size());
00217 EXPECT_EQ(3, diff2.getChanges().size());
00218
00219 diff1b.clearChanges();
00220
00221 EXPECT_EQ(3, diff1.getChanges().size());
00222 EXPECT_EQ(0, diff1b.getChanges().size());
00223 EXPECT_EQ(3, diff2.getChanges().size());
00224
00225 diff1.setWorld(world2);
00226
00227 EXPECT_EQ(6, diff1.getChanges().size());
00228 EXPECT_EQ(0, diff1b.getChanges().size());
00229 EXPECT_EQ(3, diff2.getChanges().size());
00230
00231 it = diff1.getChanges().find("objA1");
00232 EXPECT_NE(diff1.end(), it);
00233 EXPECT_EQ(collision_detection::World::DESTROY, it->second);
00234
00235 it = diff1.getChanges().find("objA2");
00236 EXPECT_NE(diff1.end(), it);
00237 EXPECT_EQ(collision_detection::World::DESTROY, it->second);
00238
00239 it = diff1.getChanges().find("objA2");
00240 EXPECT_NE(diff1.end(), it);
00241 EXPECT_EQ(collision_detection::World::DESTROY, it->second);
00242
00243 it = diff1.getChanges().find("objB1");
00244 EXPECT_NE(diff1.end(), it);
00245 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
00246
00247 it = diff1.getChanges().find("objB2");
00248 EXPECT_NE(diff1.end(), it);
00249 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
00250
00251 it = diff1.getChanges().find("objB3");
00252 EXPECT_NE(diff1.end(), it);
00253 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
00254
00255 diff1b.setWorld(world2);
00256
00257 EXPECT_EQ(6, diff1.getChanges().size());
00258 EXPECT_EQ(6, diff1b.getChanges().size());
00259 EXPECT_EQ(3, diff2.getChanges().size());
00260
00261 it = diff1b.getChanges().find("objA1");
00262 EXPECT_NE(diff1b.end(), it);
00263 EXPECT_EQ(collision_detection::World::DESTROY, it->second);
00264
00265 it = diff1b.getChanges().find("objA2");
00266 EXPECT_NE(diff1b.end(), it);
00267 EXPECT_EQ(collision_detection::World::DESTROY, it->second);
00268
00269 it = diff1b.getChanges().find("objA2");
00270 EXPECT_NE(diff1b.end(), it);
00271 EXPECT_EQ(collision_detection::World::DESTROY, it->second);
00272
00273 it = diff1b.getChanges().find("objB1");
00274 EXPECT_NE(diff1b.end(), it);
00275 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
00276
00277 it = diff1b.getChanges().find("objB2");
00278 EXPECT_NE(diff1b.end(), it);
00279 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
00280
00281 it = diff1b.getChanges().find("objB3");
00282 EXPECT_NE(diff1b.end(), it);
00283 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE, it->second);
00284
00285 world1->addToObject("objC", box, Eigen::Affine3d::Identity());
00286
00287 EXPECT_EQ(6, diff1.getChanges().size());
00288 EXPECT_EQ(6, diff1b.getChanges().size());
00289 EXPECT_EQ(3, diff2.getChanges().size());
00290
00291 world2->addToObject("objC", box, Eigen::Affine3d::Identity());
00292
00293 EXPECT_EQ(7, diff1.getChanges().size());
00294 EXPECT_EQ(7, diff1b.getChanges().size());
00295 EXPECT_EQ(4, diff2.getChanges().size());
00296
00297 diff2.setWorld(world1);
00298
00299 EXPECT_EQ(7, diff1.getChanges().size());
00300 EXPECT_EQ(7, diff1b.getChanges().size());
00301 EXPECT_EQ(7, diff2.getChanges().size());
00302
00303 it = diff2.getChanges().find("objC");
00304 EXPECT_NE(diff2.end(), it);
00305 EXPECT_EQ(collision_detection::World::CREATE | collision_detection::World::ADD_SHAPE |
00306 collision_detection::World::DESTROY,
00307 it->second);
00308
00309 world1->addToObject("objD", box, Eigen::Affine3d::Identity());
00310
00311 EXPECT_EQ(7, diff1.getChanges().size());
00312 EXPECT_EQ(7, diff1b.getChanges().size());
00313 EXPECT_EQ(8, diff2.getChanges().size());
00314
00315 world2->addToObject("objE", box, Eigen::Affine3d::Identity());
00316
00317 EXPECT_EQ(8, diff1.getChanges().size());
00318 EXPECT_EQ(8, diff1b.getChanges().size());
00319 EXPECT_EQ(8, diff2.getChanges().size());
00320 }
00321
00322 int main(int argc, char** argv)
00323 {
00324 testing::InitGoogleTest(&argc, argv);
00325 return RUN_ALL_TESTS();
00326 }