Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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 <moveit/test_resources/config.h>
00042 #include <boost/filesystem/path.hpp>
00043
00044
00045 boost::shared_ptr<urdf::ModelInterface> loadRobotModel()
00046 {
00047 std::string xml_string;
00048 std::fstream xml_file((boost::filesystem::path(MOVEIT_TEST_RESOURCES_DIR) / "urdf/robot.xml").string().c_str(), std::fstream::in);
00049 EXPECT_TRUE(xml_file.is_open());
00050 while ( xml_file.good() )
00051 {
00052 std::string line;
00053 std::getline( xml_file, line);
00054 xml_string += (line + "\n");
00055 }
00056 xml_file.close();
00057 return urdf::parseURDF(xml_string);
00058 }
00059
00060 TEST(PlanningScene, LoadRestore)
00061 {
00062 boost::shared_ptr<urdf::ModelInterface> urdf_model = loadRobotModel();
00063 boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
00064 planning_scene::PlanningScene ps(urdf_model, srdf_model);
00065 moveit_msgs::PlanningScene ps_msg;
00066 ps.getPlanningSceneMsg(ps_msg);
00067 ps.setPlanningSceneMsg(ps_msg);
00068 }
00069
00070 TEST(PlanningScene, LoadRestoreDiff)
00071 {
00072 boost::shared_ptr<urdf::ModelInterface> urdf_model = loadRobotModel();
00073 boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
00074
00075 planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
00076
00077 collision_detection::World &world = *ps->getWorldNonConst();
00078 Eigen::Affine3d id = Eigen::Affine3d::Identity();
00079 world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
00080
00081 moveit_msgs::PlanningScene ps_msg;
00082 EXPECT_TRUE(planning_scene::PlanningScene::isEmpty(ps_msg));
00083 ps->getPlanningSceneMsg(ps_msg);
00084 ps->setPlanningSceneMsg(ps_msg);
00085 EXPECT_FALSE(planning_scene::PlanningScene::isEmpty(ps_msg));
00086 EXPECT_TRUE(world.hasObject("sphere"));
00087
00088 planning_scene::PlanningScenePtr next = ps->diff();
00089 EXPECT_TRUE(next->getWorld()->hasObject("sphere"));
00090 next->getWorldNonConst()->addToObject("sphere2", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id);
00091 EXPECT_EQ(next->getWorld()->size(), 2);
00092 EXPECT_EQ(ps->getWorld()->size(), 1);
00093 next->getPlanningSceneDiffMsg(ps_msg);
00094 EXPECT_EQ(ps_msg.world.collision_objects.size(), 1);
00095 next->decoupleParent();
00096 moveit_msgs::PlanningScene ps_msg2;
00097 next->getPlanningSceneDiffMsg(ps_msg2);
00098 EXPECT_EQ(ps_msg2.world.collision_objects.size(), 0);
00099 next->getPlanningSceneMsg(ps_msg);
00100 EXPECT_EQ(ps_msg.world.collision_objects.size(), 2);
00101 ps->setPlanningSceneMsg(ps_msg);
00102 EXPECT_EQ(ps->getWorld()->size(), 2);
00103 }
00104
00105 TEST(PlanningScene, MakeAttachedDiff)
00106 {
00107 boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
00108 boost::shared_ptr<urdf::ModelInterface> urdf_model = loadRobotModel();
00109
00110 planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
00111
00112 collision_detection::World &world = *ps->getWorldNonConst();
00113 Eigen::Affine3d id = Eigen::Affine3d::Identity();
00114 world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
00115
00116 planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff();
00117
00118 moveit_msgs::AttachedCollisionObject att_obj;
00119 att_obj.link_name = "r_wrist_roll_link";
00120 att_obj.object.operation = moveit_msgs::CollisionObject::ADD;
00121 att_obj.object.id = "sphere";
00122
00123 collision_detection::CollisionRequest req;
00124 collision_detection::CollisionResult res;
00125
00126 attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj);
00127 attached_object_diff_scene->checkCollision(req,res);
00128 ps->checkCollision(req, res);
00129 }
00130
00131 int main(int argc, char **argv)
00132 {
00133 testing::InitGoogleTest(&argc, argv);
00134 return RUN_ALL_TESTS();
00135 }