test_world.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 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.h>
00039 #include <boost/bind.hpp>
00040 
00041 
00042 TEST(World, AddRemoveShape)
00043 {
00044   collision_detection::World world;
00045 
00046 
00047   // Create some shapes
00048   shapes::ShapePtr ball(new shapes::Sphere(1.0));
00049   shapes::ShapePtr box(new shapes::Box(1,2,3));
00050   shapes::ShapePtr cyl(new shapes::Cylinder(4,5));
00051 
00052   EXPECT_EQ(1, ball.use_count());
00053 
00054   EXPECT_FALSE(world.hasObject("ball"));
00055 
00056   // Add ball object
00057   world.addToObject("ball",
00058                     ball,
00059                     Eigen::Affine3d::Identity());
00060 
00061   EXPECT_EQ(2, ball.use_count());
00062   EXPECT_TRUE(world.hasObject("ball"));
00063 
00064   bool move_ok = world.moveShapeInObject(
00065                           "ball",
00066                           ball,
00067                           Eigen::Affine3d(Eigen::Translation3d(0,0,9)));
00068   EXPECT_TRUE(move_ok);
00069 
00070   EXPECT_EQ(2, ball.use_count());
00071   EXPECT_TRUE(world.hasObject("ball"));
00072 
00073   bool rm_nonexistant = world.removeShapeFromObject("xyz", ball);
00074   EXPECT_FALSE(rm_nonexistant);
00075 
00076   bool rm_wrong_shape = world.removeShapeFromObject("ball", box);
00077   EXPECT_FALSE(rm_wrong_shape);
00078 
00079   EXPECT_EQ(2, ball.use_count());
00080   EXPECT_EQ(1, box.use_count());
00081 
00082   // remove ball object
00083   bool rm_ball = world.removeShapeFromObject("ball", ball);
00084   EXPECT_TRUE(rm_ball);
00085 
00086   EXPECT_EQ(1, ball.use_count());
00087   EXPECT_FALSE(world.hasObject("ball"));
00088 
00089 
00090   // add ball again
00091   world.addToObject("ball",
00092                     ball,
00093                     Eigen::Affine3d::Identity());
00094 
00095   EXPECT_EQ(2, ball.use_count());
00096   EXPECT_TRUE(world.hasObject("ball"));
00097 
00098 
00099   EXPECT_FALSE(world.hasObject("mix1"));
00100 
00101   {
00102     std::vector<shapes::ShapeConstPtr> shapes;
00103     EigenSTL::vector_Affine3d poses;
00104 
00105     shapes.push_back(box);
00106     shapes.push_back(cyl);
00107     shapes.push_back(ball);
00108 
00109     poses.push_back(Eigen::Affine3d(Eigen::Translation3d(0,0,1)));
00110     poses.push_back(Eigen::Affine3d(Eigen::Translation3d(0,0,2)));
00111     poses.push_back(Eigen::Affine3d(Eigen::Translation3d(0,0,3)));
00112 
00113     EXPECT_FALSE(world.hasObject("mix1"));
00114 
00115     // add mix1 object
00116     world.addToObject("mix1", shapes, poses);
00117   }
00118 
00119   EXPECT_TRUE(world.hasObject("mix1"));
00120 
00121   EXPECT_EQ(2, box.use_count());
00122   EXPECT_EQ(2, cyl.use_count());
00123   EXPECT_EQ(3, ball.use_count());
00124 
00125   // add ball2
00126   world.addToObject("ball2",
00127                     ball,
00128                     Eigen::Affine3d(Eigen::Translation3d(0,0,4)));
00129 
00130   EXPECT_EQ(2, box.use_count());
00131   EXPECT_EQ(2, cyl.use_count());
00132   EXPECT_EQ(4, ball.use_count());
00133 
00134   bool rm_cyl = world.removeShapeFromObject("mix1", cyl);
00135   EXPECT_TRUE(rm_cyl);
00136 
00137   EXPECT_EQ(2, box.use_count());
00138   EXPECT_EQ(1, cyl.use_count());
00139   EXPECT_EQ(4, ball.use_count());
00140 
00141   rm_cyl = world.removeShapeFromObject("mix1", cyl);
00142   EXPECT_FALSE(rm_cyl);
00143 
00144   EXPECT_EQ(2, box.use_count());
00145   EXPECT_EQ(1, cyl.use_count());
00146   EXPECT_EQ(4, ball.use_count());
00147 
00148   EXPECT_TRUE(world.hasObject("mix1"));
00149 
00150   EXPECT_EQ(3, world.size());
00151 
00152   {
00153     collision_detection::World::ObjectConstPtr obj = world.getObject("mix1");
00154     EXPECT_EQ(2, obj.use_count());
00155 
00156     ASSERT_EQ(2, obj->shapes_.size());
00157     ASSERT_EQ(2, obj->shape_poses_.size());
00158 
00159     // check translation.z of pose
00160     EXPECT_EQ(1.0, obj->shape_poses_[0](2,3));
00161     EXPECT_EQ(3.0, obj->shape_poses_[1](2,3));
00162 
00163 
00164     move_ok = world.moveShapeInObject(
00165                             "mix1",
00166                             ball,
00167                             Eigen::Affine3d(Eigen::Translation3d(0,0,5)));
00168     EXPECT_TRUE(move_ok);
00169 
00170     collision_detection::World::ObjectConstPtr obj2 = world.getObject("mix1");
00171     EXPECT_EQ(2, obj2.use_count());
00172     EXPECT_EQ(1, obj.use_count());
00173 
00174     EXPECT_EQ(1.0, obj2->shape_poses_[0](2,3));
00175     EXPECT_EQ(5.0, obj2->shape_poses_[1](2,3));
00176 
00177     EXPECT_EQ(1.0, obj->shape_poses_[0](2,3));
00178     EXPECT_EQ(3.0, obj->shape_poses_[1](2,3));
00179 
00180     // moving object causes copy, thus extra references to shapes in obj
00181     EXPECT_EQ(3, box.use_count());
00182     EXPECT_EQ(1, cyl.use_count());
00183     EXPECT_EQ(5, ball.use_count());
00184 
00185     world.removeObject("mix1");
00186 
00187     EXPECT_EQ(2, world.size());
00188 
00189     // no change since obj2 still holds a ref
00190     EXPECT_EQ(3, box.use_count());
00191     EXPECT_EQ(1, cyl.use_count());
00192     EXPECT_EQ(5, ball.use_count());
00193 
00194     EXPECT_FALSE(world.hasObject("mix1"));
00195     EXPECT_TRUE(world.hasObject("ball2"));
00196 
00197     // ask for nonexistant object
00198     collision_detection::World::ObjectConstPtr obj3 = world.getObject("abc");
00199     EXPECT_FALSE(obj3);
00200   }
00201 
00202   EXPECT_EQ(1, box.use_count());
00203   EXPECT_EQ(1, cyl.use_count());
00204   EXPECT_EQ(3, ball.use_count());
00205 
00206   EXPECT_EQ(2, world.size());
00207 
00208   world.clearObjects();
00209 
00210   EXPECT_EQ(1, box.use_count());
00211   EXPECT_EQ(1, cyl.use_count());
00212   EXPECT_EQ(1, ball.use_count());
00213 
00214   EXPECT_FALSE(world.hasObject("mix1"));
00215   EXPECT_FALSE(world.hasObject("ball"));
00216   EXPECT_FALSE(world.hasObject("ball2"));
00217 
00218   EXPECT_EQ(0, world.size());
00219 }
00220 
00221 /* structure to hold copy of callback args */
00222 struct TestAction {
00223   collision_detection::World::Object obj_;
00224   collision_detection::World::Action action_;
00225   int cnt_;
00226   TestAction() : obj_(""), cnt_(0)
00227   {
00228     reset();
00229   }
00230   void reset()
00231   {
00232     obj_.id_ = "";
00233     obj_.shapes_.clear();
00234     obj_.shape_poses_.clear();
00235     action_ = 0x7f;
00236   }
00237 };
00238 
00239 /* notification callback */
00240 static void TrackChangesNotify(
00241               TestAction *ta,
00242               const collision_detection::World::ObjectConstPtr& obj,
00243               collision_detection::World::Action action)
00244 {
00245   ta->obj_ = *obj;
00246   ta->action_ = action;
00247   ta->cnt_++;
00248 }
00249 
00250 TEST(World, TrackChanges)
00251 {
00252   collision_detection::World world;
00253 
00254   TestAction ta;
00255   collision_detection::World::ObserverHandle observer_ta;
00256   observer_ta = world.addObserver(boost::bind(TrackChangesNotify, &ta, _1, _2));
00257 
00258 
00259 
00260 
00261   // Create some shapes
00262   shapes::ShapePtr ball(new shapes::Sphere(1.0));
00263   shapes::ShapePtr box(new shapes::Box(1,2,3));
00264   shapes::ShapePtr cyl(new shapes::Cylinder(4,5));
00265 
00266 
00267   world.addToObject("obj1",
00268                     ball,
00269                     Eigen::Affine3d::Identity());
00270 
00271   EXPECT_EQ(1, ta.cnt_);
00272   EXPECT_EQ("obj1", ta.obj_.id_);
00273   EXPECT_EQ(collision_detection::World::CREATE |
00274             collision_detection::World::ADD_SHAPE,
00275             ta.action_);
00276   ta.reset();
00277 
00278   bool move_ok = world.moveShapeInObject(
00279                           "obj1",
00280                           ball,
00281                           Eigen::Affine3d(Eigen::Translation3d(0,0,1)));
00282   EXPECT_TRUE(move_ok);
00283 
00284   EXPECT_EQ(2, ta.cnt_);
00285   EXPECT_EQ("obj1", ta.obj_.id_);
00286   EXPECT_EQ(collision_detection::World::MOVE_SHAPE,
00287             ta.action_);
00288   ta.reset();
00289 
00290   world.addToObject("obj1",
00291                     box,
00292                     Eigen::Affine3d::Identity());
00293 
00294   EXPECT_EQ(3, ta.cnt_);
00295   EXPECT_EQ("obj1", ta.obj_.id_);
00296   EXPECT_EQ(collision_detection::World::ADD_SHAPE,
00297             ta.action_);
00298   ta.reset();
00299 
00300   TestAction ta2;
00301   collision_detection::World::ObserverHandle observer_ta2;
00302   observer_ta2 = world.addObserver(boost::bind(TrackChangesNotify, &ta2, _1, _2));
00303 
00304   world.addToObject("obj2",
00305                     cyl,
00306                     Eigen::Affine3d::Identity());
00307 
00308   EXPECT_EQ(4, ta.cnt_);
00309   EXPECT_EQ("obj2", ta.obj_.id_);
00310   EXPECT_EQ(collision_detection::World::CREATE |
00311             collision_detection::World::ADD_SHAPE,
00312             ta.action_);
00313   ta.reset();
00314   EXPECT_EQ(1, ta2.cnt_);
00315   EXPECT_EQ("obj2", ta2.obj_.id_);
00316   EXPECT_EQ(collision_detection::World::CREATE |
00317             collision_detection::World::ADD_SHAPE,
00318             ta2.action_);
00319   ta2.reset();
00320 
00321 
00322   world.addToObject("obj3",
00323                     box,
00324                     Eigen::Affine3d::Identity());
00325 
00326   EXPECT_EQ(5, ta.cnt_);
00327   EXPECT_EQ("obj3", ta.obj_.id_);
00328   EXPECT_EQ(collision_detection::World::CREATE |
00329             collision_detection::World::ADD_SHAPE,
00330             ta.action_);
00331   ta.reset();
00332   EXPECT_EQ(2, ta2.cnt_);
00333   EXPECT_EQ("obj3", ta2.obj_.id_);
00334   EXPECT_EQ(collision_detection::World::CREATE |
00335             collision_detection::World::ADD_SHAPE,
00336             ta2.action_);
00337   ta2.reset();
00338 
00339   // remove nonexistent obj
00340   bool rm_bad = world.removeShapeFromObject("xyz", ball);
00341   EXPECT_FALSE(rm_bad);
00342   EXPECT_EQ(5, ta.cnt_);
00343   EXPECT_EQ(2, ta2.cnt_);
00344 
00345   // remove wrong shape
00346   rm_bad = world.removeShapeFromObject("obj2", ball);
00347   EXPECT_FALSE(rm_bad);
00348   EXPECT_EQ(5, ta.cnt_);
00349   EXPECT_EQ(2, ta2.cnt_);
00350 
00351   TestAction ta3;
00352   collision_detection::World::ObserverHandle observer_ta3;
00353   observer_ta3 = world.addObserver(boost::bind(TrackChangesNotify, &ta3, _1, _2));
00354 
00355   bool rm_good = world.removeShapeFromObject("obj2", cyl);
00356   EXPECT_TRUE(rm_good);
00357 
00358   EXPECT_EQ(6, ta.cnt_);
00359   EXPECT_EQ("obj2", ta.obj_.id_);
00360   EXPECT_EQ(collision_detection::World::DESTROY,
00361             ta.action_);
00362   ta.reset();
00363   EXPECT_EQ(3, ta2.cnt_);
00364   EXPECT_EQ("obj2", ta2.obj_.id_);
00365   EXPECT_EQ(collision_detection::World::DESTROY,
00366             ta2.action_);
00367   ta2.reset();
00368   EXPECT_EQ(1, ta3.cnt_);
00369   EXPECT_EQ("obj2", ta3.obj_.id_);
00370   EXPECT_EQ(collision_detection::World::DESTROY,
00371             ta3.action_);
00372   ta3.reset();
00373 
00374   world.removeObserver(observer_ta2);
00375 
00376   rm_good = world.removeShapeFromObject("obj1", ball);
00377   EXPECT_TRUE(rm_good);
00378 
00379   EXPECT_EQ(7, ta.cnt_);
00380   EXPECT_EQ("obj1", ta.obj_.id_);
00381   EXPECT_EQ(collision_detection::World::REMOVE_SHAPE,
00382             ta.action_);
00383   ta.reset();
00384   EXPECT_EQ(3, ta2.cnt_);
00385 
00386   EXPECT_EQ(2, ta3.cnt_);
00387   EXPECT_EQ("obj1", ta3.obj_.id_);
00388   EXPECT_EQ(collision_detection::World::REMOVE_SHAPE,
00389             ta3.action_);
00390   ta3.reset();
00391 
00392   // remove all 2 objects (should make 2 DESTROY callbacks per ta)
00393   world.clearObjects();
00394 
00395   EXPECT_EQ(9, ta.cnt_);
00396   EXPECT_EQ(collision_detection::World::DESTROY,
00397             ta.action_);
00398   ta.reset();
00399   EXPECT_EQ(3, ta2.cnt_);
00400 
00401   EXPECT_EQ(4, ta3.cnt_);
00402   EXPECT_EQ(collision_detection::World::DESTROY,
00403             ta3.action_);
00404   ta3.reset();
00405 
00406   world.removeObserver(observer_ta);
00407   world.removeObserver(observer_ta3);
00408 
00409   EXPECT_EQ(9, ta.cnt_);
00410   EXPECT_EQ(3, ta2.cnt_);
00411   EXPECT_EQ(4, ta3.cnt_);
00412 
00413   world.addToObject("obj4",
00414                     box,
00415                     Eigen::Affine3d::Identity());
00416 
00417   EXPECT_EQ(9, ta.cnt_);
00418   EXPECT_EQ(3, ta2.cnt_);
00419   EXPECT_EQ(4, ta3.cnt_);
00420 }
00421 
00422 int main(int argc, char **argv)
00423 {
00424   testing::InitGoogleTest(&argc, argv);
00425   return RUN_ALL_TESTS();
00426 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:47