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 <sstream>
42 #include <string>
43 #include <boost/filesystem/path.hpp>
44 #include <ros/package.h>
45 
46 // This function needs to return void so the gtest FAIL() macro inside
47 // it works right.
48 void loadModelFile(std::string filename, std::string& file_content)
49 {
50  boost::filesystem::path res_path(ros::package::getPath("moveit_resources"));
51  std::string xml_string;
52  std::fstream xml_file((res_path / filename).string().c_str(), std::fstream::in);
53  EXPECT_TRUE(xml_file.is_open());
54  while (xml_file.good())
55  {
56  std::string line;
57  std::getline(xml_file, line);
58  xml_string += (line + "\n");
59  }
60  xml_file.close();
61  file_content = xml_string;
62 }
63 
64 void loadRobotModel(urdf::ModelInterfaceSharedPtr& robot_model_out)
65 {
66  std::string xml_string;
67  loadModelFile("pr2_description/urdf/robot.xml", xml_string);
68  robot_model_out = urdf::parseURDF(xml_string);
69 }
70 void loadRobotModels(urdf::ModelInterfaceSharedPtr& robot_model_out, srdf::ModelSharedPtr& srdf_model_out)
71 {
72  loadRobotModel(robot_model_out);
73  std::string xml_string;
74  loadModelFile("pr2_description/srdf/robot.xml", xml_string);
75  srdf_model_out->initString(*robot_model_out, xml_string);
76 }
77 
78 TEST(PlanningScene, LoadRestore)
79 {
80  urdf::ModelInterfaceSharedPtr urdf_model;
81  loadRobotModel(urdf_model);
82  srdf::ModelSharedPtr srdf_model(new srdf::Model());
83  planning_scene::PlanningScene ps(urdf_model, srdf_model);
84  moveit_msgs::PlanningScene ps_msg;
85  ps.getPlanningSceneMsg(ps_msg);
86  EXPECT_EQ(ps.getName(), ps_msg.name);
87  EXPECT_EQ(ps.getRobotModel()->getName(), ps_msg.robot_model_name);
88  ps.setPlanningSceneMsg(ps_msg);
89  EXPECT_EQ(ps.getName(), ps_msg.name);
90  EXPECT_EQ(ps.getRobotModel()->getName(), ps_msg.robot_model_name);
91 }
92 
93 TEST(PlanningScene, LoadRestoreDiff)
94 {
95  urdf::ModelInterfaceSharedPtr urdf_model;
96  loadRobotModel(urdf_model);
97  srdf::ModelSharedPtr srdf_model(new srdf::Model());
98 
99  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
100 
101  collision_detection::World& world = *ps->getWorldNonConst();
102  Eigen::Isometry3d id = Eigen::Isometry3d::Identity();
103  world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
104 
105  moveit_msgs::PlanningScene ps_msg;
106  ps_msg.robot_state.is_diff = true;
108  ps->getPlanningSceneMsg(ps_msg);
109  ps->setPlanningSceneMsg(ps_msg);
111  EXPECT_TRUE(world.hasObject("sphere"));
112 
113  planning_scene::PlanningScenePtr next = ps->diff();
114  EXPECT_TRUE(next->getWorld()->hasObject("sphere"));
115  next->getWorldNonConst()->addToObject("sphere2", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id);
116  EXPECT_EQ(next->getWorld()->size(), 2u);
117  EXPECT_EQ(ps->getWorld()->size(), 1u);
118  next->getPlanningSceneDiffMsg(ps_msg);
119  EXPECT_EQ(ps_msg.world.collision_objects.size(), 1u);
120  next->decoupleParent();
121  moveit_msgs::PlanningScene ps_msg2;
122  next->getPlanningSceneDiffMsg(ps_msg2);
123  EXPECT_EQ(ps_msg2.world.collision_objects.size(), 0u);
124  next->getPlanningSceneMsg(ps_msg);
125  EXPECT_EQ(ps_msg.world.collision_objects.size(), 2u);
126  ps->setPlanningSceneMsg(ps_msg);
127  EXPECT_EQ(ps->getWorld()->size(), 2u);
128 }
129 
130 TEST(PlanningScene, MakeAttachedDiff)
131 {
132  srdf::ModelSharedPtr srdf_model(new srdf::Model());
133  urdf::ModelInterfaceSharedPtr urdf_model;
134  loadRobotModel(urdf_model);
135 
136  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
137 
138  collision_detection::World& world = *ps->getWorldNonConst();
139  Eigen::Isometry3d id = Eigen::Isometry3d::Identity();
140  world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
141 
142  planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff();
143 
144  moveit_msgs::AttachedCollisionObject att_obj;
145  att_obj.link_name = "r_wrist_roll_link";
146  att_obj.object.operation = moveit_msgs::CollisionObject::ADD;
147  att_obj.object.id = "sphere";
148 
151 
152  attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj);
153  attached_object_diff_scene->checkCollision(req, res);
154  ps->checkCollision(req, res);
155 }
156 
157 TEST(PlanningScene, isStateValid)
158 {
159  srdf::ModelSharedPtr srdf_model(new srdf::Model());
160  urdf::ModelInterfaceSharedPtr urdf_model;
161  loadRobotModels(urdf_model, srdf_model);
162 
163  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
164  robot_state::RobotState current_state = ps->getCurrentState();
165  if (ps->isStateColliding(current_state, "left_arm"))
166  {
167  EXPECT_FALSE(ps->isStateValid(current_state, "left_arm"));
168  }
169 }
170 
171 TEST(PlanningScene, loadGoodSceneGeometry)
172 {
173  srdf::ModelSharedPtr srdf_model(new srdf::Model());
174  urdf::ModelInterfaceSharedPtr urdf_model;
175  loadRobotModels(urdf_model, srdf_model);
176  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
177 
178  std::istringstream good_scene_geometry;
179  good_scene_geometry.str("foobar_scene\n"
180  "* foo\n"
181  "1\n"
182  "box\n"
183  "2.58 1.36 0.31\n"
184  "1.49257 1.00222 0.170051\n"
185  "0 0 4.16377e-05 1\n"
186  "0 0 1 0.3\n"
187  "* bar\n"
188  "1\n"
189  "cylinder\n"
190  "0.02 0.0001\n"
191  "0.453709 0.499136 0.355051\n"
192  "0 0 4.16377e-05 1\n"
193  "1 0 0 1\n"
194  ".\n");
195  EXPECT_TRUE(ps->loadGeometryFromStream(good_scene_geometry));
196  EXPECT_EQ(ps->getName(), "foobar_scene");
197  EXPECT_TRUE(ps->getWorld()->hasObject("foo"));
198  EXPECT_TRUE(ps->getWorld()->hasObject("bar"));
199  EXPECT_FALSE(ps->getWorld()->hasObject("baz")); // Sanity check.
200 }
201 
202 TEST(PlanningScene, loadBadSceneGeometry)
203 {
204  srdf::ModelSharedPtr srdf_model(new srdf::Model());
205  urdf::ModelInterfaceSharedPtr urdf_model;
206  loadRobotModels(urdf_model, srdf_model);
207  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
208  std::istringstream empty_scene_geometry;
209 
210  // This should fail since there is no planning scene name and no end of geometry marker.
211  EXPECT_FALSE(ps->loadGeometryFromStream(empty_scene_geometry));
212 
213  std::istringstream malformed_scene_geometry;
214  malformed_scene_geometry.str("malformed_scene_geometry\n"
215  "* foo\n"
216  "1\n"
217  "box\n"
218  "2.58 1.36\n" /* Only two tokens; should be 3 */
219  "1.49257 1.00222 0.170051\n"
220  "0 0 4.16377e-05 1\n"
221  "0 0 1 0.3\n"
222  ".\n");
223  EXPECT_FALSE(ps->loadGeometryFromStream(malformed_scene_geometry));
224 }
225 
226 int main(int argc, char** argv)
227 {
228  testing::InitGoogleTest(&argc, argv);
229  return RUN_ALL_TESTS();
230 }
bool hasObject(const std::string &object_id) const
Check if a particular object exists in the collision world.
Definition: world.cpp:134
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
TEST(PlanningScene, LoadRestore)
const robot_model::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
const std::string & getName() const
Get the name of the planning scene. This is empty by default.
void addToObject(const std::string &object_id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &poses)
Add shapes to an object in the map. This function makes repeated calls to addToObjectInternal() to ad...
Definition: world.cpp:64
#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.
void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
void loadModelFile(std::string filename, std::string &file_content)
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...
ROSLIB_DECL std::string getPath(const std::string &package_name)
std::shared_ptr< Model > ModelSharedPtr
void loadRobotModels(urdf::ModelInterfaceSharedPtr &robot_model_out, srdf::ModelSharedPtr &srdf_model_out)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
#define EXPECT_TRUE(args)
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Jul 11 2020 03:51:21