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 <boost/filesystem/path.hpp>
00042 #include <ros/package.h>
00043
00044
00045
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 }