test_aabb.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, 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 the 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: Martin Pecka */
36 
40 #include <urdf_parser/urdf_parser.h>
41 #include <fstream>
42 #include <boost/filesystem.hpp>
43 #include <gtest/gtest.h>
45 #include <tf2/LinearMath/Vector3.h>
47 
48 // To visualize bbox of the PR2, set this to 1.
49 #ifndef VISUALIZE_PR2_RVIZ
50 #define VISUALIZE_PR2_RVIZ 0
51 #endif
52 
53 #if VISUALIZE_PR2_RVIZ
54 #include <ros/ros.h>
55 #include <visualization_msgs/Marker.h>
57 #endif
58 
59 class TestAABB : public testing::Test
60 {
61 protected:
62  void SetUp() override{};
63 
64  robot_state::RobotState loadModel(const std::string& robot_name)
65  {
66  robot_model::RobotModelPtr model = moveit::core::loadTestingRobotModel(robot_name);
67  return loadModel(model);
68  }
69 
70  robot_state::RobotState loadModel(const robot_model::RobotModelPtr& model)
71  {
73  robot_state.setToDefaultValues();
74  robot_state.update(true);
75 
76  return robot_state;
77  }
78 
79  void TearDown() override
80  {
81  }
82 };
83 
84 TEST_F(TestAABB, TestPR2)
85 {
86  // Contains a link with mesh geometry that is not centered
87 
88  robot_state::RobotState pr2_state = this->loadModel("pr2");
89 
90  const Eigen::Vector3d& extentsBaseFootprint = pr2_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin();
91  // values taken from moveit_resources/pr2_description/urdf/robot.xml
92  EXPECT_NEAR(extentsBaseFootprint[0], 0.001, 1e-4);
93  EXPECT_NEAR(extentsBaseFootprint[1], 0.001, 1e-4);
94  EXPECT_NEAR(extentsBaseFootprint[2], 0.001, 1e-4);
95 
96  const Eigen::Vector3d& offsetBaseFootprint = pr2_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset();
97  EXPECT_NEAR(offsetBaseFootprint[0], 0.0, 1e-4);
98  EXPECT_NEAR(offsetBaseFootprint[1], 0.0, 1e-4);
99  EXPECT_NEAR(offsetBaseFootprint[2], 0.071, 1e-4);
100 
101  const Eigen::Vector3d& extentsBaseLink = pr2_state.getLinkModel("base_link")->getShapeExtentsAtOrigin();
102  // values computed from moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl in e.g. Meshlab
103  EXPECT_NEAR(extentsBaseLink[0], 0.668242, 1e-4);
104  EXPECT_NEAR(extentsBaseLink[1], 0.668242, 1e-4);
105  EXPECT_NEAR(extentsBaseLink[2], 0.656175, 1e-4);
106 
107  const Eigen::Vector3d& offsetBaseLink = pr2_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset();
108  EXPECT_NEAR(offsetBaseLink[0], 0.0, 1e-4);
109  EXPECT_NEAR(offsetBaseLink[1], 0.0, 1e-4);
110  EXPECT_NEAR(offsetBaseLink[2], 0.656175 / 2, 1e-4); // The 3D mesh isn't centered, but is whole above z axis
111 
112  std::vector<double> pr2_aabb;
113  pr2_state.computeAABB(pr2_aabb);
114  ASSERT_EQ(pr2_aabb.size(), 6u);
115 
116  EXPECT_NEAR(pr2_aabb[0], -0.3376, 1e-4);
117  EXPECT_NEAR(pr2_aabb[1], 0.6499, 1e-4);
118  EXPECT_NEAR(pr2_aabb[2], -0.6682 / 2, 1e-4);
119  EXPECT_NEAR(pr2_aabb[3], 0.6682 / 2, 1e-4);
120  EXPECT_NEAR(pr2_aabb[4], 0.0044, 1e-4);
121  EXPECT_NEAR(pr2_aabb[5], 1.6328, 1e-4);
122 
123  // Test a specific link known to have some global rotation in the default pose
124 
125  const moveit::core::LinkModel* link = pr2_state.getLinkModel("l_forearm_link");
126  Eigen::Isometry3d transform = pr2_state.getGlobalLinkTransform(link); // intentional copy, we will translate
127  const Eigen::Vector3d& extents = link->getShapeExtentsAtOrigin();
128  transform.translate(link->getCenteredBoundingBoxOffset());
129  moveit::core::AABB aabb;
130  aabb.extendWithTransformedBox(transform, extents);
131 
132  EXPECT_NEAR(aabb.center()[0], 0.5394, 1e-4);
133  EXPECT_NEAR(aabb.center()[1], 0.1880, 1e-4);
134  EXPECT_NEAR(aabb.center()[2], 1.1665, 1e-4);
135  EXPECT_NEAR(aabb.sizes()[0], 0.2209, 1e-4);
136  EXPECT_NEAR(aabb.sizes()[1], 0.1201, 1e-4);
137  EXPECT_NEAR(aabb.sizes()[2], 0.2901, 1e-4);
138 
139 #if VISUALIZE_PR2_RVIZ
140  std::cout << "Overall bounding box of PR2:" << std::endl;
141  std::string dims[] = { "x", "y", "z" };
142  for (std::size_t i = 0; i < 3; ++i)
143  {
144  double dim = pr2_aabb[2 * i + 1] - pr2_aabb[2 * i];
145  double center = dim / 2;
146  std::cout << dims[i] << ": size=" << dim << ", offset=" << (pr2_aabb[2 * i + 1] - center) << std::endl;
147  }
148 
149  // Initialize a ROS publisher
150  char* argv[0];
151  int argc = 0;
152  ros::init(argc, argv, "visualize_pr2");
153  ros::NodeHandle nh;
154  ros::Publisher pub_aabb = nh.advertise<visualization_msgs::Marker>("/visualization_aabb", 10);
155  ros::Publisher pub_obb = nh.advertise<visualization_msgs::Marker>("/visualization_obb", 10);
156 
157  // Wait for the publishers to establish connections
158  sleep(5);
159 
160  // Prepare the ROS message we will reuse throughout the rest of the function.
161  visualization_msgs::Marker msg;
162  msg.header.frame_id = pr2_state.getRobotModel()->getRootLinkName();
163  msg.type = visualization_msgs::Marker::CUBE;
164  msg.color.a = 0.5;
165  msg.lifetime.sec = 3000;
166 
167  // Publish the AABB of the whole model
168  msg.ns = "pr2";
169  msg.pose.position.x = (pr2_aabb[0] + pr2_aabb[1]) / 2;
170  msg.pose.position.y = (pr2_aabb[2] + pr2_aabb[3]) / 2;
171  msg.pose.position.z = (pr2_aabb[4] + pr2_aabb[5]) / 2;
172  msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0;
173  msg.pose.orientation.w = 1;
174  msg.scale.x = pr2_aabb[1] - pr2_aabb[0];
175  msg.scale.y = pr2_aabb[3] - pr2_aabb[2];
176  msg.scale.z = pr2_aabb[5] - pr2_aabb[4];
177  pub_aabb.publish(msg);
178 
179  // Publish BBs for all links
180  std::vector<const moveit::core::LinkModel*> links = pr2_state.getRobotModel()->getLinkModelsWithCollisionGeometry();
181  for (std::size_t i = 0; i < links.size(); ++i)
182  {
183  Eigen::Isometry3d transform = pr2_state.getGlobalLinkTransform(links[i]); // intentional copy, we will translate
184  const Eigen::Vector3d& extents = links[i]->getShapeExtentsAtOrigin();
185  transform.translate(links[i]->getCenteredBoundingBoxOffset());
186  moveit::core::AABB aabb;
187  aabb.extendWithTransformedBox(transform, extents);
188 
189  // Publish AABB
190  msg.ns = links[i]->getName();
191  msg.pose.position.x = transform.translation()[0];
192  msg.pose.position.y = transform.translation()[1];
193  msg.pose.position.z = transform.translation()[2];
194  msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0;
195  msg.pose.orientation.w = 1;
196  msg.color.r = 1;
197  msg.color.b = 0;
198  msg.scale.x = aabb.sizes()[0];
199  msg.scale.y = aabb.sizes()[1];
200  msg.scale.z = aabb.sizes()[2];
201  pub_aabb.publish(msg);
202 
203  // Publish OBB (oriented BB)
204  msg.ns += "-obb";
205  msg.pose.position.x = transform.translation()[0];
206  msg.pose.position.y = transform.translation()[1];
207  msg.pose.position.z = transform.translation()[2];
208  msg.scale.x = extents[0];
209  msg.scale.y = extents[1];
210  msg.scale.z = extents[2];
211  msg.color.r = 0;
212  msg.color.b = 1;
213  Eigen::Quaterniond q(transform.rotation());
214  msg.pose.orientation.x = q.x();
215  msg.pose.orientation.y = q.y();
216  msg.pose.orientation.z = q.z();
217  msg.pose.orientation.w = q.w();
218  pub_obb.publish(msg);
219  }
220 
221  // Publish BBs for all attached bodies
222  std::vector<const moveit::core::AttachedBody*> attached_bodies;
223  pr2_state.getAttachedBodies(attached_bodies);
224  for (std::vector<const moveit::core::AttachedBody*>::const_iterator it = attached_bodies.begin();
225  it != attached_bodies.end(); ++it)
226  {
227  const EigenSTL::vector_Isometry3d& transforms = (*it)->getGlobalCollisionBodyTransforms();
228  const std::vector<shapes::ShapeConstPtr>& shapes = (*it)->getShapes();
229  for (std::size_t i = 0; i < transforms.size(); ++i)
230  {
231  Eigen::Vector3d extents = shapes::computeShapeExtents(shapes[i].get());
232  moveit::core::AABB aabb;
233  aabb.extendWithTransformedBox(transforms[i], extents);
234 
235  // Publish AABB
236  msg.ns = (*it)->getName() + boost::lexical_cast<std::string>(i);
237  msg.pose.position.x = transforms[i].translation()[0];
238  msg.pose.position.y = transforms[i].translation()[1];
239  msg.pose.position.z = transforms[i].translation()[2];
240  msg.pose.orientation.x = msg.pose.orientation.y = msg.pose.orientation.z = 0;
241  msg.pose.orientation.w = 1;
242  msg.color.r = 1;
243  msg.color.b = 0;
244  msg.scale.x = aabb.sizes()[0];
245  msg.scale.y = aabb.sizes()[1];
246  msg.scale.z = aabb.sizes()[2];
247  pub_aabb.publish(msg);
248 
249  // Publish OBB (oriented BB)
250  msg.ns += "-obb";
251  msg.pose.position.x = transforms[i].translation()[0];
252  msg.pose.position.y = transforms[i].translation()[1];
253  msg.pose.position.z = transforms[i].translation()[2];
254  msg.scale.x = extents[0];
255  msg.scale.y = extents[1];
256  msg.scale.z = extents[2];
257  msg.color.r = 0;
258  msg.color.b = 1;
259  Eigen::Quaterniond q(transforms[i].rotation());
260  msg.pose.orientation.x = q.x();
261  msg.pose.orientation.y = q.y();
262  msg.pose.orientation.z = q.z();
263  msg.pose.orientation.w = q.w();
264  pub_obb.publish(msg);
265  }
266  }
267 #endif
268 }
269 
270 TEST_F(TestAABB, TestSimple)
271 {
272  // Contains a link with simple geometry and an offset in the collision link
273  moveit::core::RobotModelBuilder builder("simple", "base_footprint");
274  geometry_msgs::Pose origin;
275  tf2::toMsg(tf2::Vector3(0, 0, 0.051), origin.position);
276  origin.orientation.w = 1.0;
277  builder.addChain("base_footprint->base_link", "fixed", { origin });
278 
279  tf2::toMsg(tf2::Vector3(0, 0, 0), origin.position);
280  builder.addCollisionMesh("base_link", "package://moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl",
281  origin);
282 
283  tf2::toMsg(tf2::Vector3(0, 0, 0.071), origin.position);
284  builder.addCollisionBox("base_footprint", { 0.001, 0.001, 0.001 }, origin);
285 
286  builder.addVirtualJoint("odom_combined", "base_footprint", "planar", "world_joint");
287  builder.addGroup({}, { "world_joint" }, "base");
288 
289  ASSERT_TRUE(builder.isValid());
290  robot_state::RobotState simple_state = loadModel(builder.build());
291  std::vector<double> simple_aabb;
292  simple_state.computeAABB(simple_aabb);
293 
294  ASSERT_EQ(simple_aabb.size(), 6u);
295  EXPECT_NEAR(simple_aabb[0], -0.6682 / 2, 1e-4);
296  EXPECT_NEAR(simple_aabb[1], 0.6682 / 2, 1e-4);
297  EXPECT_NEAR(simple_aabb[2], -0.6682 / 2, 1e-4);
298  EXPECT_NEAR(simple_aabb[3], 0.6682 / 2, 1e-4);
299  EXPECT_NEAR(simple_aabb[4], 0.0510, 1e-4);
300  EXPECT_NEAR(simple_aabb[5], 0.7071, 1e-4);
301 }
302 
303 TEST_F(TestAABB, TestComplex)
304 {
305  // Contains a link with simple geometry and an offset and rotation in the collision link
306  moveit::core::RobotModelBuilder builder("complex", "base_footprint");
307  geometry_msgs::Pose origin;
308  tf2::toMsg(tf2::Vector3(0, 0, 1.0), origin.position);
309  tf2::Quaternion q;
310  q.setRPY(0, 0, 1.5708);
311  origin.orientation = tf2::toMsg(q);
312  builder.addChain("base_footprint->base_link", "fixed", { origin });
313  tf2::toMsg(tf2::Vector3(5.0, 0, 1.0), origin.position);
314  builder.addCollisionBox("base_link", { 1.0, 0.1, 0.1 }, origin);
315  tf2::toMsg(tf2::Vector3(4.0, 0, 1.0), origin.position);
316  builder.addCollisionBox("base_link", { 1.0, 0.1, 0.1 }, origin);
317  tf2::toMsg(tf2::Vector3(-5.0, 0.0, -1.0), origin.position);
318  q.setRPY(0, 1.5708, 0);
319  origin.orientation = tf2::toMsg(q);
320  builder.addCollisionBox("base_footprint", { 0.1, 1.0, 0.1 }, origin);
321  builder.addVirtualJoint("odom_combined", "base_footprint", "planar", "world_joint");
322  builder.addGroup({}, { "world_joint" }, "base");
323 
324  ASSERT_TRUE(builder.isValid());
325  robot_state::RobotState complex_state = this->loadModel(builder.build());
326 
327  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[0], 0.1, 1e-4);
328  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[1], 1.0, 1e-4);
329  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getShapeExtentsAtOrigin()[2], 0.1, 1e-4);
330  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset()[0], -5.0, 1e-4);
331  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset()[1], 0.0, 1e-4);
332  EXPECT_NEAR(complex_state.getLinkModel("base_footprint")->getCenteredBoundingBoxOffset()[2], -1.0, 1e-4);
333 
334  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getShapeExtentsAtOrigin()[0], 1.1, 1e-4);
335  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getShapeExtentsAtOrigin()[1], 1.0, 1e-4);
336  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getShapeExtentsAtOrigin()[2], 0.1, 1e-4);
337  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset()[0], 4.5, 1e-4);
338  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset()[1], 0.0, 1e-4);
339  EXPECT_NEAR(complex_state.getLinkModel("base_link")->getCenteredBoundingBoxOffset()[2], 1.0, 1e-4);
340 
341  std::vector<double> complex_aabb;
342  complex_state.computeAABB(complex_aabb);
343 
344  ASSERT_EQ(complex_aabb.size(), 6u);
345  EXPECT_NEAR(complex_aabb[0], -5.05, 1e-4);
346  EXPECT_NEAR(complex_aabb[1], 0.5, 1e-4);
347  EXPECT_NEAR(complex_aabb[2], -0.5, 1e-4);
348  EXPECT_NEAR(complex_aabb[3], 5.05, 1e-4);
349  EXPECT_NEAR(complex_aabb[4], -1.05, 1e-4);
350  EXPECT_NEAR(complex_aabb[5], 2.05, 1e-4);
351 }
352 
353 int main(int argc, char** argv)
354 {
355  testing::InitGoogleTest(&argc, argv);
356  return RUN_ALL_TESTS();
357 }
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
Core components of MoveIt!
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
const Eigen::Vector3d & getShapeExtentsAtOrigin() const
Get the extents of the link&#39;s geometry (dimensions of axis-aligned bounding box around all shapes tha...
Definition: link_model.h:184
void addChain(const std::string &section, const std::string &type, const std::vector< geometry_msgs::Pose > &joint_origins={})
Adds a chain of links and joints to the builder. The joint names are generated automatically as "<par...
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
robot_state::RobotState loadModel(const std::string &robot_name)
Definition: test_aabb.cpp:64
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void addGroup(const std::vector< std::string > &links, const std::vector< std::string > &joints, const std::string &name)
Adds a new group using a list of links and a list of joints.
#define EXPECT_NEAR(a, b, prec)
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
void computeAABB(std::vector< double > &aabb) const
Compute an axis-aligned bounding box that contains the current state. The format for aabb is (minx...
void TearDown() override
Definition: test_aabb.cpp:79
void getAttachedBodies(std::vector< const AttachedBody *> &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
void publish(const boost::shared_ptr< M > &message) const
void update(bool force=false)
Update all transforms.
void addVirtualJoint(const std::string &parent_frame, const std::string &child_link, const std::string &type, const std::string &name="")
Adds a virtual joint to the SRDF.
int main(int argc, char **argv)
Definition: test_aabb.cpp:353
const RobotModelConstPtr & getRobotModel() const
Get the robot model this state is constructed for.
Definition: robot_state.h:138
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
B toMsg(const A &a)
bool isValid()
Returns true if the building process so far has been valid.
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:156
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Definition: robot_state.h:1419
void extendWithTransformedBox(const Eigen::Isometry3d &transform, const Eigen::Vector3d &box)
Extend with a box transformed by the given transform.
Definition: aabb.cpp:39
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
robot_state::RobotState loadModel(const robot_model::RobotModelPtr &model)
Definition: test_aabb.cpp:70
Eigen::Vector3d computeShapeExtents(const ShapeMsg &shape_msg)
void SetUp() override
Definition: test_aabb.cpp:62
void addCollisionBox(const std::string &link_name, const std::vector< double > &dims, geometry_msgs::Pose origin)
Adds a collision box to a specific link.
TEST_F(TestAABB, TestPR2)
Definition: test_aabb.cpp:84
void addCollisionMesh(const std::string &link_name, const std::string &filename, geometry_msgs::Pose origin)
Adds a collision mesh to a specific link.
Represents an axis-aligned bounding box.
Definition: aabb.h:47
const Eigen::Vector3d & getCenteredBoundingBoxOffset() const
Get the offset of the center of the bounding box of this link when the link is positioned at origin...
Definition: link_model.h:190
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Oct 11 2019 03:56:04