moveit_core: test_world_diff.cpp Source File

Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Acorn Pooley */
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   // Create some shapes
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 }