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 <string>
42 #include <boost/filesystem/path.hpp>
43 #include <moveit_resources/config.h>
44 
45 // This function needs to return void so the gtest FAIL() macro inside
46 // it works right.
47 void loadModelFile(std::string filename, std::string& file_content)
48 {
49  boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
50  std::string xml_string;
51  std::fstream xml_file((res_path / filename).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  file_content = xml_string;
61 }
62 
63 void loadRobotModel(urdf::ModelInterfaceSharedPtr& robot_model_out)
64 {
65  std::string xml_string;
66  loadModelFile("pr2_description/urdf/robot.xml", xml_string);
67  robot_model_out = urdf::parseURDF(xml_string);
68 }
69 void loadRobotModels(urdf::ModelInterfaceSharedPtr& robot_model_out, srdf::ModelSharedPtr& srdf_model_out)
70 {
71  loadRobotModel(robot_model_out);
72  std::string xml_string;
73  loadModelFile("pr2_description/srdf/robot.xml", xml_string);
74  srdf_model_out->initString(*robot_model_out, xml_string);
75 }
76 
77 TEST(PlanningScene, LoadRestore)
78 {
79  urdf::ModelInterfaceSharedPtr urdf_model;
80  loadRobotModel(urdf_model);
81  srdf::ModelSharedPtr srdf_model(new srdf::Model());
82  planning_scene::PlanningScene ps(urdf_model, srdf_model);
83  moveit_msgs::PlanningScene ps_msg;
84  ps.getPlanningSceneMsg(ps_msg);
85  EXPECT_EQ(ps.getName(), ps_msg.name);
86  EXPECT_EQ(ps.getRobotModel()->getName(), ps_msg.robot_model_name);
87  ps.setPlanningSceneMsg(ps_msg);
88  EXPECT_EQ(ps.getName(), ps_msg.name);
89  EXPECT_EQ(ps.getRobotModel()->getName(), ps_msg.robot_model_name);
90 }
91 
92 TEST(PlanningScene, LoadRestoreDiff)
93 {
94  urdf::ModelInterfaceSharedPtr urdf_model;
95  loadRobotModel(urdf_model);
96  srdf::ModelSharedPtr srdf_model(new srdf::Model());
97 
98  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
99 
100  collision_detection::World& world = *ps->getWorldNonConst();
101  Eigen::Affine3d id = Eigen::Affine3d::Identity();
102  world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
103 
104  moveit_msgs::PlanningScene ps_msg;
105  ps_msg.robot_state.is_diff = true;
107  ps->getPlanningSceneMsg(ps_msg);
108  ps->setPlanningSceneMsg(ps_msg);
110  EXPECT_TRUE(world.hasObject("sphere"));
111 
112  planning_scene::PlanningScenePtr next = ps->diff();
113  EXPECT_TRUE(next->getWorld()->hasObject("sphere"));
114  next->getWorldNonConst()->addToObject("sphere2", shapes::ShapeConstPtr(new shapes::Sphere(0.5)), id);
115  EXPECT_EQ(next->getWorld()->size(), 2);
116  EXPECT_EQ(ps->getWorld()->size(), 1);
117  next->getPlanningSceneDiffMsg(ps_msg);
118  EXPECT_EQ(ps_msg.world.collision_objects.size(), 1);
119  next->decoupleParent();
120  moveit_msgs::PlanningScene ps_msg2;
121  next->getPlanningSceneDiffMsg(ps_msg2);
122  EXPECT_EQ(ps_msg2.world.collision_objects.size(), 0);
123  next->getPlanningSceneMsg(ps_msg);
124  EXPECT_EQ(ps_msg.world.collision_objects.size(), 2);
125  ps->setPlanningSceneMsg(ps_msg);
126  EXPECT_EQ(ps->getWorld()->size(), 2);
127 }
128 
129 TEST(PlanningScene, MakeAttachedDiff)
130 {
131  srdf::ModelSharedPtr srdf_model(new srdf::Model());
132  urdf::ModelInterfaceSharedPtr urdf_model;
133  loadRobotModel(urdf_model);
134 
135  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
136 
137  collision_detection::World& world = *ps->getWorldNonConst();
138  Eigen::Affine3d id = Eigen::Affine3d::Identity();
139  world.addToObject("sphere", shapes::ShapeConstPtr(new shapes::Sphere(0.4)), id);
140 
141  planning_scene::PlanningScenePtr attached_object_diff_scene = ps->diff();
142 
143  moveit_msgs::AttachedCollisionObject att_obj;
144  att_obj.link_name = "r_wrist_roll_link";
145  att_obj.object.operation = moveit_msgs::CollisionObject::ADD;
146  att_obj.object.id = "sphere";
147 
150 
151  attached_object_diff_scene->processAttachedCollisionObjectMsg(att_obj);
152  attached_object_diff_scene->checkCollision(req, res);
153  ps->checkCollision(req, res);
154 }
155 
156 TEST(PlanningScene, isStateValid)
157 {
158  srdf::ModelSharedPtr srdf_model(new srdf::Model());
159  urdf::ModelInterfaceSharedPtr urdf_model;
160  loadRobotModels(urdf_model, srdf_model);
161 
162  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(urdf_model, srdf_model));
163  robot_state::RobotState current_state = ps->getCurrentState();
164  if (ps->isStateColliding(current_state, "left_arm"))
165  {
166  EXPECT_FALSE(ps->isStateValid(current_state, "left_arm"));
167  }
168 }
169 
170 int main(int argc, char** argv)
171 {
172  testing::InitGoogleTest(&argc, argv);
173  return RUN_ALL_TESTS();
174 }
const std::string & getName() const
Get the name of the planning scene. This is empty by default.
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:64
TEST(PlanningScene, LoadRestore)
bool hasObject(const std::string &id) const
Check if a particular object exists in the collision world.
Definition: world.cpp:134
#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.
const robot_model::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
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...
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:123
#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 Oct 15 2018 03:44:47