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 <moveit_resources/config.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   boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
00049 
00050   std::string xml_string;
00051   std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in);
00052   EXPECT_TRUE(xml_file.is_open());
00053   while (xml_file.good())
00054   {
00055     std::string line;
00056     std::getline(xml_file, line);
00057     xml_string += (line + "\n");
00058   }
00059   xml_file.close();
00060   robot_model_out = urdf::parseURDF(xml_string);
00061 }
00062 
00063 TEST(PlanningScene, LoadRestore)
00064 {
00065   boost::shared_ptr<urdf::ModelInterface> urdf_model;
00066   loadRobotModel(urdf_model);
00067   boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
00068   planning_scene::PlanningScene ps(urdf_model, srdf_model);
00069   moveit_msgs::PlanningScene ps_msg;
00070   ps.getPlanningSceneMsg(ps_msg);
00071   ps.setPlanningSceneMsg(ps_msg);
00072 }
00073 
00074 TEST(PlanningScene, LoadRestoreDiff)
00075 {
00076   boost::shared_ptr<urdf::ModelInterface> urdf_model;
00077   loadRobotModel(urdf_model);
00078   boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
00079 
00080   planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
00081 
00082   collision_detection::World& world = *ps->getWorldNonConst();
00083   Eigen::Affine3d id = Eigen::Affine3d::Identity();
00084   world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
00085 
00086   moveit_msgs::PlanningScene ps_msg;
00087   ps_msg.robot_state.is_diff = true;
00088   EXPECT_TRUE(planning_scene::PlanningScene::isEmpty(ps_msg));
00089   ps->getPlanningSceneMsg(ps_msg);
00090   ps->setPlanningSceneMsg(ps_msg);
00091   EXPECT_FALSE(planning_scene::PlanningScene::isEmpty(ps_msg));
00092   EXPECT_TRUE(world.hasObject("sphere"));
00093 
00094   planning_scene::PlanningScenePtr next = ps->diff();
00095   EXPECT_TRUE(next->getWorld()->hasObject("sphere"));
00096   next->getWorldNonConst()->addToObject("sphere2", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id);
00097   EXPECT_EQ(next->getWorld()->size(), 2);
00098   EXPECT_EQ(ps->getWorld()->size(), 1);
00099   next->getPlanningSceneDiffMsg(ps_msg);
00100   EXPECT_EQ(ps_msg.world.collision_objects.size(), 1);
00101   next->decoupleParent();
00102   moveit_msgs::PlanningScene ps_msg2;
00103   next->getPlanningSceneDiffMsg(ps_msg2);
00104   EXPECT_EQ(ps_msg2.world.collision_objects.size(), 0);
00105   next->getPlanningSceneMsg(ps_msg);
00106   EXPECT_EQ(ps_msg.world.collision_objects.size(), 2);
00107   ps->setPlanningSceneMsg(ps_msg);
00108   EXPECT_EQ(ps->getWorld()->size(), 2);
00109 }
00110 
00111 TEST(PlanningScene, MakeAttachedDiff)
00112 {
00113   boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
00114   boost::shared_ptr<urdf::ModelInterface> urdf_model;
00115   loadRobotModel(urdf_model);
00116 
00117   planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
00118 
00119   collision_detection::World& world = *ps->getWorldNonConst();
00120   Eigen::Affine3d id = Eigen::Affine3d::Identity();
00121   world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
00122 
00123   planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff();
00124 
00125   moveit_msgs::AttachedCollisionObject att_obj;
00126   att_obj.link_name = "r_wrist_roll_link";
00127   att_obj.object.operation = moveit_msgs::CollisionObject::ADD;
00128   att_obj.object.id = "sphere";
00129 
00130   collision_detection::CollisionRequest req;
00131   collision_detection::CollisionResult res;
00132 
00133   attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj);
00134   attached_object_diff_scene->checkCollision(req, res);
00135   ps->checkCollision(req, res);
00136 }
00137 
00138 int main(int argc, char** argv)
00139 {
00140   testing::InitGoogleTest(&argc, argv);
00141   return RUN_ALL_TESTS();
00142 }


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