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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jan 17 2018 03:31:36