test_planning_scene.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include <gtest/gtest.h>
39 #include <urdf_parser/urdf_parser.h>
40 #include <fstream>
41 #include <boost/filesystem/path.hpp>
42 #include <moveit_resources/config.h>
43 
44 // This function needs to return void so the gtest FAIL() macro inside
45 // it works right.
46 void loadRobotModel(urdf::ModelInterfaceSharedPtr& robot_model_out)
47 {
48  boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
49 
50  std::string xml_string;
51  std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in);
52  EXPECT_TRUE(xml_file.is_open());
53  while (xml_file.good())
54  {
55  std::string line;
56  std::getline(xml_file, line);
57  xml_string += (line + "\n");
58  }
59  xml_file.close();
60  robot_model_out = urdf::parseURDF(xml_string);
61 }
62 
63 TEST(PlanningScene, LoadRestore)
64 {
65  urdf::ModelInterfaceSharedPtr urdf_model;
66  loadRobotModel(urdf_model);
67  srdf::ModelSharedPtr srdf_model(new srdf::Model());
68  planning_scene::PlanningScene ps(urdf_model, srdf_model);
69  moveit_msgs::PlanningScene ps_msg;
70  ps.getPlanningSceneMsg(ps_msg);
71  ps.setPlanningSceneMsg(ps_msg);
72 }
73 
74 TEST(PlanningScene, LoadRestoreDiff)
75 {
76  urdf::ModelInterfaceSharedPtr urdf_model;
77  loadRobotModel(urdf_model);
78  srdf::ModelSharedPtr srdf_model(new srdf::Model());
79 
80  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
81 
82  collision_detection::World& world = *ps->getWorldNonConst();
83  Eigen::Affine3d id = Eigen::Affine3d::Identity();
84  world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
85 
86  moveit_msgs::PlanningScene ps_msg;
87  ps_msg.robot_state.is_diff = true;
89  ps->getPlanningSceneMsg(ps_msg);
90  ps->setPlanningSceneMsg(ps_msg);
92  EXPECT_TRUE(world.hasObject("sphere"));
93 
94  planning_scene::PlanningScenePtr next = ps->diff();
95  EXPECT_TRUE(next->getWorld()->hasObject("sphere"));
96  next->getWorldNonConst()->addToObject("sphere2", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id);
97  EXPECT_EQ(next->getWorld()->size(), 2);
98  EXPECT_EQ(ps->getWorld()->size(), 1);
99  next->getPlanningSceneDiffMsg(ps_msg);
100  EXPECT_EQ(ps_msg.world.collision_objects.size(), 1);
101  next->decoupleParent();
102  moveit_msgs::PlanningScene ps_msg2;
103  next->getPlanningSceneDiffMsg(ps_msg2);
104  EXPECT_EQ(ps_msg2.world.collision_objects.size(), 0);
105  next->getPlanningSceneMsg(ps_msg);
106  EXPECT_EQ(ps_msg.world.collision_objects.size(), 2);
107  ps->setPlanningSceneMsg(ps_msg);
108  EXPECT_EQ(ps->getWorld()->size(), 2);
109 }
110 
111 TEST(PlanningScene, MakeAttachedDiff)
112 {
113  srdf::ModelSharedPtr srdf_model(new srdf::Model());
114  urdf::ModelInterfaceSharedPtr urdf_model;
115  loadRobotModel(urdf_model);
116 
117  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
118 
119  collision_detection::World& world = *ps->getWorldNonConst();
120  Eigen::Affine3d id = Eigen::Affine3d::Identity();
121  world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
122 
123  planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff();
124 
125  moveit_msgs::AttachedCollisionObject att_obj;
126  att_obj.link_name = "r_wrist_roll_link";
127  att_obj.object.operation = moveit_msgs::CollisionObject::ADD;
128  att_obj.object.id = "sphere";
129 
132 
133  attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj);
134  attached_object_diff_scene->checkCollision(req, res);
135  ps->checkCollision(req, res);
136 }
137 
138 int main(int argc, char** argv)
139 {
140  testing::InitGoogleTest(&argc, argv);
141  return RUN_ALL_TESTS();
142 }
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
Check if a message includes any information about a planning scene, or it is just a default...
void loadRobotModel(urdf::ModelInterfaceSharedPtr &robot_model_out)
int main(int argc, char **argv)
Maintain a representation of the environment.
Definition: world.h:60
void addToObject(const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &poses)
Add shapes to an object in the map. This function makes repeated calls to addToObjectInternal() to ad...
Definition: world.cpp:62
TEST(PlanningScene, LoadRestore)
bool hasObject(const std::string &id) const
Check if a particular object exists in the collision world.
Definition: world.cpp:133
#define EXPECT_FALSE(args)
#define EXPECT_EQ(a, b)
This class maintains the representation of the environment as seen by a planning instance. The environment geometry, the robot geometry and state are maintained.
def xml_string(rootXml, addHeader=True)
bool setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Set this instance of a planning scene to be the same as the one serialized in the scene message...
#define EXPECT_TRUE(args)
void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44