test_fcl_collision_detection.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, 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 
00037 #include <moveit/test_resources/config.h>
00038 #include <moveit/robot_model/robot_model.h>
00039 #include <moveit/robot_state/robot_state.h>
00040 #include <moveit/collision_detection_fcl/collision_world_fcl.h>
00041 #include <moveit/collision_detection_fcl/collision_robot_fcl.h>
00042 
00043 #include <urdf_parser/urdf_parser.h>
00044 #include <geometric_shapes/shape_operations.h>
00045 
00046 #include <gtest/gtest.h>
00047 #include <sstream>
00048 #include <algorithm>
00049 #include <ctype.h>
00050 #include <fstream>
00051 
00052 #include <boost/filesystem.hpp>
00053 
00054 typedef collision_detection::CollisionWorldFCL DefaultCWorldType;
00055 typedef collision_detection::CollisionRobotFCL DefaultCRobotType;
00056 
00057 static std::string urdf_file = (boost::filesystem::path(MOVEIT_TEST_RESOURCES_DIR) / "urdf/robot.xml").string();
00058 static std::string srdf_file = (boost::filesystem::path(MOVEIT_TEST_RESOURCES_DIR) / "srdf/robot.xml").string();
00059 static std::string kinect_dae_file = (boost::filesystem::path(MOVEIT_TEST_RESOURCES_DIR) / "urdf/meshes/sensors/kinect_v0/kinect.dae").string();
00060 
00061 class FclCollisionDetectionTester : public testing::Test
00062 {
00063 
00064 protected:
00065 
00066   virtual void SetUp()
00067   {
00068     srdf_model_.reset(new srdf::Model());
00069     std::string xml_string;
00070     std::fstream xml_file(urdf_file.c_str(), std::fstream::in);
00071 
00072     if (xml_file.is_open())
00073     {
00074       while ( xml_file.good() )
00075       {
00076         std::string line;
00077         std::getline( xml_file, line);
00078         xml_string += (line + "\n");
00079       }
00080       xml_file.close();
00081       urdf_model_ = urdf::parseURDF(xml_string);
00082       urdf_ok_ = urdf_model_;
00083     }
00084     else
00085     {
00086       EXPECT_EQ("FAILED TO OPEN FILE", urdf_file);
00087       urdf_ok_ = false;
00088     }
00089     srdf_ok_ = srdf_model_->initFile(*urdf_model_, srdf_file);
00090 
00091     kmodel_.reset(new robot_model::RobotModel(urdf_model_, srdf_model_));
00092 
00093     acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
00094 
00095     crobot_.reset(new DefaultCRobotType(kmodel_));
00096     cworld_.reset(new DefaultCWorldType());
00097   }
00098 
00099   virtual void TearDown()
00100   {
00101 
00102   }
00103 
00104 protected:
00105 
00106   bool urdf_ok_;
00107   bool srdf_ok_;
00108 
00109   boost::shared_ptr<urdf::ModelInterface>  urdf_model_;
00110   boost::shared_ptr<srdf::Model>           srdf_model_;
00111 
00112   robot_model::RobotModelPtr               kmodel_;
00113 
00114   boost::shared_ptr<collision_detection::CollisionRobot>        crobot_;
00115   boost::shared_ptr<collision_detection::CollisionWorld>        cworld_;
00116 
00117   collision_detection::AllowedCollisionMatrixPtr acm_;
00118 
00119 };
00120 
00121 
00122 TEST_F(FclCollisionDetectionTester, InitOK)
00123 {
00124   ASSERT_TRUE(urdf_ok_);
00125   ASSERT_TRUE(srdf_ok_);
00126 }
00127 
00128 TEST_F(FclCollisionDetectionTester, DefaultNotInCollision)
00129 {
00130   robot_state::RobotState kstate(kmodel_);
00131   kstate.setToDefaultValues();
00132 
00133   collision_detection::CollisionRequest req;
00134   collision_detection::CollisionResult res;
00135   crobot_->checkSelfCollision(req, res, kstate, *acm_);
00136   ASSERT_FALSE(res.collision);
00137 }
00138 
00139 
00140 TEST_F(FclCollisionDetectionTester, LinksInCollision)
00141 {
00142   collision_detection::CollisionRequest req;
00143   collision_detection::CollisionResult res1;
00144   collision_detection::CollisionResult res2;
00145   collision_detection::CollisionResult res3;
00146   //req.contacts = true;
00147   //req.max_contacts = 100;
00148 
00149   robot_state::RobotState kstate(kmodel_);
00150   kstate.setToDefaultValues();
00151 
00152   Eigen::Affine3d offset = Eigen::Affine3d::Identity();
00153   offset.translation().x() = .01;
00154 
00155   //  kstate.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
00156   //  kstate.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
00157   kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
00158   kstate.updateStateWithLinkAt("base_bellow_link", offset);
00159 
00160   acm_->setEntry("base_link", "base_bellow_link", false);
00161   crobot_->checkSelfCollision(req, res1, kstate, *acm_);
00162   ASSERT_TRUE(res1.collision);
00163 
00164   acm_->setEntry("base_link", "base_bellow_link", true);
00165   crobot_->checkSelfCollision(req, res2, kstate, *acm_);
00166   ASSERT_FALSE(res2.collision);
00167 
00168   //  req.verbose = true;
00169   //  kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
00170   //  kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
00171   kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
00172   kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);
00173 
00174   acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
00175   crobot_->checkSelfCollision(req, res3, kstate, *acm_);
00176   ASSERT_TRUE(res3.collision);
00177 }
00178 
00179 
00180 TEST_F(FclCollisionDetectionTester, ContactReporting)
00181 {
00182   collision_detection::CollisionRequest req;
00183   req.contacts = true;
00184   req.max_contacts = 1;
00185 
00186   robot_state::RobotState kstate(kmodel_);
00187   kstate.setToDefaultValues();
00188 
00189   Eigen::Affine3d offset = Eigen::Affine3d::Identity();
00190   offset.translation().x() = .01;
00191 
00192   //  kstate.getLinkState("base_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
00193   //  kstate.getLinkState("base_bellow_link")->updateGivenGlobalLinkTransform(offset);
00194   //  kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(Eigen::Affine3d::Identity());
00195   //  kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(offset);
00196 
00197   kstate.updateStateWithLinkAt("base_link", Eigen::Affine3d::Identity());
00198   kstate.updateStateWithLinkAt("base_bellow_link", offset);
00199   kstate.updateStateWithLinkAt("r_gripper_palm_link", Eigen::Affine3d::Identity());
00200   kstate.updateStateWithLinkAt("l_gripper_palm_link", offset);
00201 
00202   acm_->setEntry("base_link", "base_bellow_link", false);
00203   acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
00204 
00205   collision_detection::CollisionResult res;
00206   crobot_->checkSelfCollision(req, res, kstate, *acm_);
00207   ASSERT_TRUE(res.collision);
00208   EXPECT_EQ(res.contacts.size(),1);
00209   EXPECT_EQ(res.contacts.begin()->second.size(),1);
00210 
00211   res.clear();
00212   req.max_contacts = 2;
00213   req.max_contacts_per_pair = 1;
00214   //  req.verbose = true;
00215   crobot_->checkSelfCollision(req, res, kstate, *acm_);
00216   ASSERT_TRUE(res.collision);
00217   EXPECT_EQ(res.contacts.size(), 2);
00218   EXPECT_EQ(res.contacts.begin()->second.size(),1);
00219 
00220   res.contacts.clear();
00221   res.contact_count = 0;
00222 
00223   req.max_contacts = 10;
00224   req.max_contacts_per_pair = 2;
00225   acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), false));
00226   crobot_->checkSelfCollision(req, res, kstate, *acm_);
00227   ASSERT_TRUE(res.collision);
00228   EXPECT_LE(res.contacts.size(), 10);
00229   EXPECT_LE(res.contact_count, 10);
00230 
00231 }
00232 
00233 TEST_F(FclCollisionDetectionTester, ContactPositions)
00234 {
00235   collision_detection::CollisionRequest req;
00236   req.contacts = true;
00237   req.max_contacts = 1;
00238 
00239   robot_state::RobotState kstate(kmodel_);
00240   kstate.setToDefaultValues();
00241 
00242   Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
00243   Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
00244 
00245   pos1.translation().x() = 5.0;
00246   pos2.translation().x() = 5.01;
00247 
00248   //  kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
00249   //  kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
00250   kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
00251   kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
00252 
00253   acm_->setEntry("r_gripper_palm_link", "l_gripper_palm_link", false);
00254 
00255   collision_detection::CollisionResult res;
00256   crobot_->checkSelfCollision(req, res, kstate, *acm_);
00257   ASSERT_TRUE(res.collision);
00258   ASSERT_EQ(res.contacts.size(),1);
00259   ASSERT_EQ(res.contacts.begin()->second.size(),1);
00260 
00261   for(collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
00262       it != res.contacts.end();
00263       it++) {
00264     EXPECT_NEAR(it->second[0].pos.x(), 5.0, .33);
00265   }
00266 
00267   pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond::Identity());
00268   pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond(0.965, 0.0, 0.258, 0.0));
00269   //  kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
00270   //  kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
00271   kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
00272   kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
00273 
00274   collision_detection::CollisionResult res2;
00275   crobot_->checkSelfCollision(req, res2, kstate, *acm_);
00276   ASSERT_TRUE(res2.collision);
00277   ASSERT_EQ(res2.contacts.size(),1);
00278   ASSERT_EQ(res2.contacts.begin()->second.size(),1);
00279 
00280   for(collision_detection::CollisionResult::ContactMap::const_iterator it = res2.contacts.begin();
00281       it != res2.contacts.end();
00282       it++) {
00283     EXPECT_NEAR(it->second[0].pos.x(), 3.0, 0.33);
00284   }
00285 
00286   pos1 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond::Identity());
00287   pos2 = Eigen::Affine3d(Eigen::Translation3d(3.0,0.0,0.0)*Eigen::Quaterniond(M_PI/4.0, 0.0, M_PI/4.0, 0.0));
00288   //  kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
00289   //  kstate.getLinkState("l_gripper_palm_link")->updateGivenGlobalLinkTransform(pos2);
00290   kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
00291   kstate.updateStateWithLinkAt("l_gripper_palm_link", pos2);
00292 
00293   collision_detection::CollisionResult res3;
00294   crobot_->checkSelfCollision(req, res2, kstate, *acm_);
00295   ASSERT_FALSE(res3.collision);
00296 }
00297 
00298 TEST_F(FclCollisionDetectionTester, AttachedBodyTester) {
00299   collision_detection::CollisionRequest req;
00300   collision_detection::CollisionResult res;
00301 
00302   acm_.reset(new collision_detection::AllowedCollisionMatrix(kmodel_->getLinkModelNames(), true));
00303 
00304   robot_state::RobotState kstate(kmodel_);
00305   kstate.setToDefaultValues();
00306 
00307   Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
00308   pos1.translation().x() = 5.0;
00309 
00310   //  kstate.getLinkState("r_gripper_palm_link")->updateGivenGlobalLinkTransform(pos1);
00311   kstate.updateStateWithLinkAt("r_gripper_palm_link", pos1);
00312   crobot_->checkSelfCollision(req, res, kstate, *acm_);
00313   ASSERT_FALSE(res.collision);
00314 
00315   shapes::Shape* shape = new shapes::Box(.1,.1,.1);
00316   cworld_->getWorld()->addToObject("box", shapes::ShapeConstPtr(shape), pos1);
00317 
00318   res = collision_detection::CollisionResult();
00319   cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
00320   ASSERT_TRUE(res.collision);
00321 
00322   //deletes shape
00323   cworld_->getWorld()->removeObject("box");
00324 
00325   shape = new shapes::Box(.1,.1,.1);
00326   std::vector<shapes::ShapeConstPtr> shapes;
00327   EigenSTL::vector_Affine3d poses;
00328   shapes.push_back(shapes::ShapeConstPtr(shape));
00329   poses.push_back(Eigen::Affine3d::Identity());
00330   std::vector<std::string> touch_links;
00331   kstate.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
00332 
00333   res = collision_detection::CollisionResult();
00334   crobot_->checkSelfCollision(req, res, kstate, *acm_);
00335   ASSERT_TRUE(res.collision);
00336 
00337   //deletes shape
00338   kstate.clearAttachedBody("box");
00339 
00340   touch_links.push_back("r_gripper_palm_link");
00341   shapes[0].reset(new shapes::Box(.1,.1,.1));
00342   kstate.attachBody("box", shapes, poses, touch_links, "r_gripper_palm_link");
00343 
00344   res = collision_detection::CollisionResult();
00345   crobot_->checkSelfCollision(req, res, kstate, *acm_);
00346   ASSERT_FALSE(res.collision);
00347 
00348   pos1.translation().x() = 5.01;
00349   shapes::Shape* coll = new shapes::Box(.1, .1, .1);
00350   cworld_->getWorld()->addToObject("coll", shapes::ShapeConstPtr(coll), pos1);
00351   res = collision_detection::CollisionResult();
00352   cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
00353   ASSERT_TRUE(res.collision);
00354 
00355   acm_->setEntry("coll", "r_gripper_palm_link", true);
00356   res = collision_detection::CollisionResult();
00357   cworld_->checkRobotCollision(req, res, *crobot_, kstate, *acm_);
00358   ASSERT_TRUE(res.collision);
00359 }
00360 
00361 TEST_F(FclCollisionDetectionTester, DiffSceneTester)
00362 {
00363   robot_state::RobotState kstate(kmodel_);
00364   kstate.setToDefaultValues();
00365 
00366   collision_detection::CollisionRequest req;
00367   collision_detection::CollisionResult res;
00368 
00369   collision_detection::CollisionRobotFCL new_crobot(*(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
00370 
00371   ros::WallTime before = ros::WallTime::now();
00372   new_crobot.checkSelfCollision(req,res,kstate);
00373   double first_check = (ros::WallTime::now()-before).toSec();
00374   before = ros::WallTime::now();
00375   new_crobot.checkSelfCollision(req,res,kstate);
00376   double second_check = (ros::WallTime::now()-before).toSec();
00377 
00378   EXPECT_LT(fabs(first_check-second_check), .05);
00379 
00380   std::vector<shapes::ShapeConstPtr> shapes;
00381   shapes.resize(1);
00382 
00383   boost::filesystem::path path(boost::filesystem::current_path());
00384 
00385   shapes[0].reset(shapes::createMeshFromResource("file://"+path.string()+"/"+kinect_dae_file));
00386 
00387   EigenSTL::vector_Affine3d poses;
00388   poses.push_back(Eigen::Affine3d::Identity());
00389 
00390   std::vector<std::string> touch_links;
00391   kstate.attachBody("kinect", shapes, poses, touch_links, "r_gripper_palm_link");
00392 
00393   before = ros::WallTime::now();
00394   new_crobot.checkSelfCollision(req,res,kstate);
00395   first_check = (ros::WallTime::now()-before).toSec();
00396   before = ros::WallTime::now();
00397   new_crobot.checkSelfCollision(req,res,kstate);
00398   second_check = (ros::WallTime::now()-before).toSec();
00399 
00400   //the first check is going to take a while, as data must be constructed
00401   EXPECT_LT(second_check, .1);
00402 
00403   collision_detection::CollisionRobotFCL other_new_crobot(*(dynamic_cast<collision_detection::CollisionRobotFCL*>(crobot_.get())));
00404   before = ros::WallTime::now();
00405   new_crobot.checkSelfCollision(req,res,kstate);
00406   first_check = (ros::WallTime::now()-before).toSec();
00407   before = ros::WallTime::now();
00408   new_crobot.checkSelfCollision(req,res,kstate);
00409   second_check = (ros::WallTime::now()-before).toSec();
00410 
00411   EXPECT_LT(fabs(first_check-second_check), .05);
00412 
00413 }
00414 
00415 TEST_F(FclCollisionDetectionTester, ConvertObjectToAttached)
00416 {
00417   collision_detection::CollisionRequest req;
00418   collision_detection::CollisionResult res;
00419 
00420   boost::filesystem::path path(boost::filesystem::current_path());
00421   shapes::ShapeConstPtr shape(shapes::createMeshFromResource("file://"+path.string()+"/"+kinect_dae_file));
00422   Eigen::Affine3d pos1 = Eigen::Affine3d::Identity();
00423   Eigen::Affine3d pos2 = Eigen::Affine3d::Identity();
00424   pos2.translation().x() = 10.0;
00425 
00426   cworld_->getWorld()->addToObject("kinect", shape, pos1);
00427 
00428   robot_state::RobotState kstate(kmodel_);
00429   kstate.setToDefaultValues();
00430 
00431   ros::WallTime before = ros::WallTime::now();
00432   cworld_->checkRobotCollision(req, res, *crobot_, kstate);
00433   double first_check = (ros::WallTime::now()-before).toSec();
00434   before = ros::WallTime::now();
00435   cworld_->checkRobotCollision(req, res, *crobot_, kstate);
00436   double second_check = (ros::WallTime::now()-before).toSec();
00437 
00438   EXPECT_LT(second_check, .05);
00439 
00440   collision_detection::CollisionWorld::ObjectPtr object = cworld_->getWorld()->getObject("kinect");
00441   cworld_->getWorld()->removeObject("kinect");
00442 
00443   robot_state::RobotState kstate1(kmodel_);
00444   robot_state::RobotState kstate2(kmodel_);
00445   kstate1.setToDefaultValues();
00446   kstate2.setToDefaultValues();
00447 
00448   std::vector<std::string> touch_links;
00449   kstate1.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
00450 
00451   EigenSTL::vector_Affine3d other_poses;
00452   other_poses.push_back(pos2);
00453 
00454   // This creates a new set of constant properties for the attached body, which happens to be the same as the one above;
00455   kstate2.attachBody("kinect", object->shapes_, object->shape_poses_, touch_links, "r_gripper_palm_link");
00456 
00457   //going to take a while, but that's fine
00458   res = collision_detection::CollisionResult();
00459   crobot_->checkSelfCollision(req,res,kstate1);
00460 
00461   EXPECT_TRUE(res.collision);
00462 
00463   before = ros::WallTime::now();
00464   crobot_->checkSelfCollision(req,res,kstate1, *acm_);
00465   first_check = (ros::WallTime::now()-before).toSec();
00466   before = ros::WallTime::now();
00467   req.verbose = true;
00468   res = collision_detection::CollisionResult();
00469   crobot_->checkSelfCollision(req,res,kstate2, *acm_);
00470   second_check = (ros::WallTime::now()-before).toSec();
00471 
00472   EXPECT_LT(first_check, .05);
00473   EXPECT_LT(fabs(first_check-second_check), .1);
00474 }
00475 
00476 
00477 
00478 TEST_F(FclCollisionDetectionTester, TestCollisionMapAdditionSpeed)
00479 {
00480   EigenSTL::vector_Affine3d poses;
00481   std::vector<shapes::ShapeConstPtr> shapes;
00482   for(unsigned int i = 0; i < 10000; i++) {
00483     poses.push_back(Eigen::Affine3d::Identity());
00484     shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(.01, .01, .01)));
00485   }
00486   ros::WallTime start = ros::WallTime::now();
00487   cworld_->getWorld()->addToObject("map", shapes, poses);
00488   double t = (ros::WallTime::now()-start).toSec();
00489   EXPECT_GE(1.0, t);
00490   // this is not really a failure; it is just that slow;
00491   // looking into doing collision checking with a voxel grid.
00492   logInform("Adding boxes took %g", t);
00493 }
00494 
00495 
00496 TEST_F(FclCollisionDetectionTester, MoveMesh)
00497 {
00498   robot_state::RobotState kstate1(kmodel_);
00499   kstate1.setToDefaultValues();
00500 
00501   Eigen::Affine3d kinect_pose;
00502   kinect_pose.setIdentity();
00503   shapes::ShapePtr kinect_shape;
00504   boost::filesystem::path path(boost::filesystem::current_path());
00505   kinect_shape.reset(shapes::createMeshFromResource("file://"+path.string()+"/"+kinect_dae_file));
00506 
00507   cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
00508 
00509   Eigen::Affine3d np;
00510   for(unsigned int i = 0; i < 5 ; i++)
00511   {
00512     np = Eigen::Translation3d(i*.001, i*.001, i*.001)*Eigen::Quaterniond::Identity();
00513     cworld_->getWorld()->moveShapeInObject("kinect", kinect_shape, np);
00514     collision_detection::CollisionRequest req;
00515     collision_detection::CollisionResult res;
00516     cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
00517   }
00518 
00519   // SUCCEED();
00520 }
00521 
00522 TEST_F(FclCollisionDetectionTester, TestChangingShapeSize)
00523 {
00524   robot_state::RobotState kstate1(kmodel_);
00525   kstate1.setToDefaultValues();
00526 
00527   collision_detection::CollisionRequest req1;
00528   collision_detection::CollisionResult res1;
00529 
00530 
00531   ASSERT_FALSE(res1.collision);
00532 
00533   EigenSTL::vector_Affine3d poses;
00534   std::vector<shapes::ShapeConstPtr> shapes;
00535   for(unsigned int i = 0; i < 5; i++)
00536   {
00537     cworld_->getWorld()->removeObject("shape");
00538     shapes.clear();
00539     poses.clear();
00540     shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1+i*.0001, 1+i*.0001, 1+i*.0001)));
00541     poses.push_back(Eigen::Affine3d::Identity());
00542     cworld_->getWorld()->addToObject("shape", shapes, poses);
00543     collision_detection::CollisionRequest req;
00544     collision_detection::CollisionResult res;
00545     cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
00546     ASSERT_TRUE(res.collision);
00547   }
00548 
00549   Eigen::Affine3d kinect_pose;
00550   shapes::ShapePtr kinect_shape;
00551   boost::filesystem::path path(boost::filesystem::current_path());
00552   kinect_shape.reset(shapes::createMeshFromResource("file://"+path.string()+"/"+kinect_dae_file));
00553   cworld_->getWorld()->addToObject("kinect", kinect_shape, kinect_pose);
00554   collision_detection::CollisionRequest req2;
00555   collision_detection::CollisionResult res2;
00556   cworld_->checkCollision(req2, res2, *crobot_, kstate1, *acm_);
00557   ASSERT_TRUE(res2.collision);
00558   for(unsigned int i = 0; i < 5; i++) {
00559     cworld_->getWorld()->removeObject("shape");
00560     shapes.clear();
00561     poses.clear();
00562     shapes.push_back(shapes::ShapeConstPtr(new shapes::Box(1+i*.0001, 1+i*.0001, 1+i*.0001)));
00563     poses.push_back(Eigen::Affine3d::Identity());
00564     cworld_->getWorld()->addToObject("shape", shapes, poses);
00565     collision_detection::CollisionRequest req;
00566     collision_detection::CollisionResult res;
00567     cworld_->checkCollision(req, res, *crobot_, kstate1, *acm_);
00568     ASSERT_TRUE(res.collision);
00569   }
00570 }
00571 
00572 
00573 int main(int argc, char **argv)
00574 {
00575   testing::InitGoogleTest(&argc, argv);
00576   return RUN_ALL_TESTS();
00577 }


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