test_world_diff.cpp
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 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", 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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:44