test_collision_objects.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2021, Universitaet Hamburg
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: Michael 'v4hn' Goerner */
36 
37 #include <gtest/gtest.h>
41 #include <urdf_parser/urdf_parser.h>
42 #include <fstream>
43 #include <sstream>
44 #include <string>
45 #include <boost/filesystem/path.hpp>
46 #include <ros/package.h>
47 #include <tf2_eigen/tf2_eigen.h>
48 
49 namespace
50 {
51 void makeSphere(moveit_msgs::CollisionObject& co)
52 {
53  // set valid primitive
54  shape_msgs::SolidPrimitive primitive;
55  primitive.type = shape_msgs::SolidPrimitive::SPHERE;
56  primitive.dimensions.push_back(/* SPHERE_RADIUS */ 1.0);
57  co.primitives.push_back(primitive);
58  geometry_msgs::Pose pose;
59  pose.orientation.w = 1.0;
60  co.primitive_poses.push_back(pose);
61 }
62 } // namespace
63 
64 TEST(PlanningScene, fillInObjectPoseFromPrimitive)
65 {
66  moveit::core::RobotModelPtr robot_model(moveit::core::RobotModelBuilder("empty_robot", "base_link").build());
68 
69  moveit_msgs::CollisionObject co;
70  co.header.frame_id = robot_model->getModelFrame();
71  co.id = "object_no_pose";
72  co.operation = moveit_msgs::CollisionObject::ADD;
73  makeSphere(co);
74  scene.processCollisionObjectMsg(co);
75 
76  Eigen::Isometry3d primitive_pose;
77  tf2::fromMsg(co.primitive_poses.at(0), primitive_pose);
78  ASSERT_TRUE(scene.knowsFrameTransform(co.id)) << "failed to add object";
79  EXPECT_TRUE(scene.getFrameTransform(co.id).isApprox(primitive_pose))
80  << "scene did not use only primitive pose as object pose";
81 }
82 
83 TEST(PlanningScene, fillInPrimitivePose)
84 {
85  moveit::core::RobotModelPtr robot_model(moveit::core::RobotModelBuilder("empty_robot", "base_link").build());
87 
88  moveit_msgs::CollisionObject co;
89  co.header.frame_id = robot_model->getModelFrame();
90  co.id = "object_no_primitive_pose";
91  co.operation = moveit_msgs::CollisionObject::ADD;
92  makeSphere(co);
93  co.pose = co.primitive_poses.at(0);
94  co.primitive_poses.resize(0);
95  scene.processCollisionObjectMsg(co);
96 
97  Eigen::Isometry3d object_pose;
98  tf2::fromMsg(co.pose, object_pose);
99  ASSERT_TRUE(scene.knowsFrameTransform(co.id)) << "failed to add object";
100  EXPECT_TRUE(scene.getFrameTransform(co.id).isApprox(object_pose))
101  << "scene did not implicitly fill in identity pose for only primitive";
102 }
103 
104 TEST(PlanningScene, rememberMetadataWhenAttached)
105 {
106  moveit::core::RobotModelPtr robot_model(moveit::core::RobotModelBuilder("empty_robot", "base_link").build());
108 
109  // prepare planning scene message to add a colored object
110  moveit_msgs::PlanningScene scene_msg;
111  scene_msg.robot_model_name = robot_model->getName();
112  scene_msg.is_diff = true;
113  scene_msg.robot_state.is_diff = true;
114 
115  moveit_msgs::CollisionObject co;
116  co.header.frame_id = robot_model->getModelFrame();
117  co.id = "blue_sphere";
118  co.operation = moveit_msgs::CollisionObject::ADD;
119  co.pose.orientation.w = 1.0;
120  makeSphere(co);
121 
122  // meta-data 1: object type
123  co.type.key = "blue_sphere_type";
124  co.type.db = "{'type':'CustomDB'}";
125  scene_msg.world.collision_objects.push_back(co);
126 
127  // meta-data 2: object color
128  moveit_msgs::ObjectColor color;
129  color.id = co.id;
130  color.color.b = 1.0;
131  color.color.a = 1.0;
132  scene_msg.object_colors.push_back(color);
133 
134  EXPECT_FALSE(scene.hasObjectColor(co.id)) << "scene knows color before adding it(?)";
135  EXPECT_FALSE(scene.hasObjectType(co.id)) << "scene knows type before adding it(?)";
136 
137  // add object to scene
138  scene.usePlanningSceneMsg(scene_msg);
139 
140  EXPECT_TRUE(scene.hasObjectColor(co.id)) << "scene failed to add object color";
141  EXPECT_EQ(scene.getObjectColor(co.id), color.color) << "scene added wrong object color";
142  EXPECT_TRUE(scene.hasObjectType(co.id)) << "scene failed to add object type";
143  EXPECT_EQ(scene.getObjectType(co.id), co.type) << "scene added wrong object type";
144 
145  // attach object
146  moveit_msgs::AttachedCollisionObject aco;
147  aco.object.operation = moveit_msgs::CollisionObject::ADD;
148  aco.object.id = co.id;
149  aco.link_name = robot_model->getModelFrame();
151 
152  EXPECT_EQ(scene.getObjectColor(co.id), color.color) << "scene forgot object color after it got attached";
153  EXPECT_EQ(scene.getObjectType(co.id), co.type) << "scene forgot object type after it got attached";
154 
155  // trying to remove object from the scene while it is attached is expected to fail
156  co.operation = moveit_msgs::CollisionObject::REMOVE;
158  << "scene removed attached object from collision world (although it's not there)";
159 
160  // detach again right away
161  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
163 
164  EXPECT_EQ(scene.getObjectColor(co.id), color.color) << "scene forgot specified color after attach/detach";
165  EXPECT_EQ(scene.getObjectType(co.id), co.type) << "scene forgot specified type after attach/detach";
166 }
167 
168 int main(int argc, char** argv)
169 {
170  testing::InitGoogleTest(&argc, argv);
171  return RUN_ALL_TESTS();
172 }
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
planning_scene::PlanningScene::hasObjectColor
bool hasObjectColor(const std::string &id) const
Definition: planning_scene.cpp:2021
planning_scene::PlanningScene::processAttachedCollisionObjectMsg
bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object)
Definition: planning_scene.cpp:1501
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
planning_scene::PlanningScene::getObjectType
const object_recognition_msgs::ObjectType & getObjectType(const std::string &id) const
Definition: planning_scene.cpp:1984
planning_scene.h
planning_scene::PlanningScene::getObjectColor
const std_msgs::ColorRGBA & getObjectColor(const std::string &id) const
Definition: planning_scene.cpp:2031
EXPECT_TRUE
#define EXPECT_TRUE(condition)
planning_scene::PlanningScene::knowsFrameTransform
bool knowsFrameTransform(const std::string &id) const
Check if a transform to the frame id is known. This will be known if id is a link name,...
Definition: planning_scene.cpp:1957
planning_scene::PlanningScene::hasObjectType
bool hasObjectType(const std::string &id) const
Definition: planning_scene.cpp:1974
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
planning_scene::PlanningScene
This class maintains the representation of the environment as seen by a planning instance....
Definition: planning_scene.h:122
EXPECT_FALSE
#define EXPECT_FALSE(condition)
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
ASSERT_TRUE
#define ASSERT_TRUE(condition)
package.h
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
robot_model_test_utils.h
planning_scene::PlanningScene::usePlanningSceneMsg
bool usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Call setPlanningSceneMsg() or setPlanningSceneDiffMsg() depending on how the is_diff member of the me...
Definition: planning_scene.cpp:1379
moveit::core::RobotModelBuilder
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
Definition: robot_model_test_utils.h:148
main
int main(int argc, char **argv)
Definition: test_collision_objects.cpp:168
planning_scene::PlanningScene::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &id) const
Get the transform corresponding to the frame id. This will be known if id is a link name,...
Definition: planning_scene.cpp:1926
gtest.h
planning_scene::PlanningScene::processCollisionObjectMsg
bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object)
Definition: planning_scene.cpp:1715
TEST
TEST(PlanningScene, fillInObjectPoseFromPrimitive)
Definition: test_collision_objects.cpp:64
message_checks.h


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 18 2022 02:24:25