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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Apr 23 2018 10:24:45