test_planning_scene.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, 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: Ioan Sucan */
00036 
00037 #include <gtest/gtest.h>
00038 #include <moveit/planning_scene/planning_scene.h>
00039 #include <urdf_parser/urdf_parser.h>
00040 #include <fstream>
00041 #include <boost/filesystem/path.hpp>
00042 #include <ros/package.h>
00043 
00044 // This function needs to return void so the gtest FAIL() macro inside
00045 // it works right.
00046 void loadRobotModel(boost::shared_ptr<urdf::ModelInterface>& robot_model_out)
00047 {
00048   std::string resource_dir = ros::package::getPath("moveit_resources");
00049   if(resource_dir == "")
00050   {
00051     FAIL() << "Failed to find package moveit_resources.";
00052     return;
00053   }
00054   boost::filesystem::path res_path(resource_dir);
00055 
00056   std::string xml_string;
00057   std::fstream xml_file((res_path / "test/urdf/robot.xml").string().c_str(), std::fstream::in);
00058   EXPECT_TRUE(xml_file.is_open());
00059   while ( xml_file.good() )
00060   {
00061     std::string line;
00062     std::getline( xml_file, line);
00063     xml_string += (line + "\n");
00064   }
00065   xml_file.close();
00066   robot_model_out = urdf::parseURDF(xml_string);
00067 }
00068 
00069 TEST(PlanningScene, LoadRestore)
00070 {
00071   boost::shared_ptr<urdf::ModelInterface> urdf_model;
00072   loadRobotModel(urdf_model);
00073   boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
00074   planning_scene::PlanningScene ps(urdf_model, srdf_model);
00075   moveit_msgs::PlanningScene ps_msg;
00076   ps.getPlanningSceneMsg(ps_msg);
00077   ps.setPlanningSceneMsg(ps_msg);
00078 }
00079 
00080 TEST(PlanningScene, LoadRestoreDiff)
00081 {
00082   boost::shared_ptr<urdf::ModelInterface> urdf_model;
00083   loadRobotModel(urdf_model);
00084   boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
00085 
00086   planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
00087 
00088   collision_detection::World &world = *ps->getWorldNonConst();
00089   Eigen::Affine3d id = Eigen::Affine3d::Identity();
00090   world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
00091 
00092   moveit_msgs::PlanningScene ps_msg;
00093   EXPECT_TRUE(planning_scene::PlanningScene::isEmpty(ps_msg));
00094   ps->getPlanningSceneMsg(ps_msg);
00095   ps->setPlanningSceneMsg(ps_msg);
00096   EXPECT_FALSE(planning_scene::PlanningScene::isEmpty(ps_msg));
00097   EXPECT_TRUE(world.hasObject("sphere"));
00098 
00099   planning_scene::PlanningScenePtr next = ps->diff();
00100   EXPECT_TRUE(next->getWorld()->hasObject("sphere"));
00101   next->getWorldNonConst()->addToObject("sphere2", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id);
00102   EXPECT_EQ(next->getWorld()->size(), 2);
00103   EXPECT_EQ(ps->getWorld()->size(), 1);
00104   next->getPlanningSceneDiffMsg(ps_msg);
00105   EXPECT_EQ(ps_msg.world.collision_objects.size(), 1);
00106   next->decoupleParent();
00107   moveit_msgs::PlanningScene ps_msg2;
00108   next->getPlanningSceneDiffMsg(ps_msg2);
00109   EXPECT_EQ(ps_msg2.world.collision_objects.size(), 0);
00110   next->getPlanningSceneMsg(ps_msg);
00111   EXPECT_EQ(ps_msg.world.collision_objects.size(), 2);
00112   ps->setPlanningSceneMsg(ps_msg);
00113   EXPECT_EQ(ps->getWorld()->size(), 2);
00114 }
00115 
00116 TEST(PlanningScene, MakeAttachedDiff)
00117 {
00118   boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
00119   boost::shared_ptr<urdf::ModelInterface> urdf_model;
00120   loadRobotModel(urdf_model);
00121 
00122   planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
00123 
00124   collision_detection::World &world = *ps->getWorldNonConst();
00125   Eigen::Affine3d id = Eigen::Affine3d::Identity();
00126   world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
00127 
00128   planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff();
00129 
00130   moveit_msgs::AttachedCollisionObject att_obj;
00131   att_obj.link_name = "r_wrist_roll_link";
00132   att_obj.object.operation = moveit_msgs::CollisionObject::ADD;
00133   att_obj.object.id = "sphere";
00134 
00135   collision_detection::CollisionRequest req;
00136   collision_detection::CollisionResult res;
00137 
00138   attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj);
00139   attached_object_diff_scene->checkCollision(req,res);
00140   ps->checkCollision(req, res);
00141 }
00142 
00143 int main(int argc, char **argv)
00144 {
00145   testing::InitGoogleTest(&argc, argv);
00146   return RUN_ALL_TESTS();
00147 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53