body_operations.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 
40 #include <Eigen/Geometry>
41 
43 {
44  Body* body = NULL;
45 
46  if (shape)
47  switch (shape->type)
48  {
49  case shapes::BOX:
50  body = new bodies::Box(shape);
51  break;
52  case shapes::SPHERE:
53  body = new bodies::Sphere(shape);
54  break;
55  case shapes::CYLINDER:
56  body = new bodies::Cylinder(shape);
57  break;
58  case shapes::MESH:
59  body = new bodies::ConvexMesh(shape);
60  break;
61  default:
62  CONSOLE_BRIDGE_logError("Creating body from shape: Unknown shape type %d", (int)shape->type);
63  break;
64  }
65 
66  return body;
67 }
68 
69 void bodies::mergeBoundingSpheres(const std::vector<BoundingSphere>& spheres, BoundingSphere& mergedSphere)
70 {
71  if (spheres.empty())
72  {
73  mergedSphere.center = Eigen::Vector3d(0.0, 0.0, 0.0);
74  mergedSphere.radius = 0.0;
75  }
76  else
77  {
78  mergedSphere = spheres[0];
79  for (unsigned int i = 1; i < spheres.size(); ++i)
80  {
81  if (spheres[i].radius <= 0.0)
82  continue;
83  Eigen::Vector3d diff = spheres[i].center - mergedSphere.center;
84  double d = diff.norm();
85  if (d + mergedSphere.radius <= spheres[i].radius)
86  {
87  mergedSphere.center = spheres[i].center;
88  mergedSphere.radius = spheres[i].radius;
89  }
90  else if (d + spheres[i].radius > mergedSphere.radius)
91  {
92  Eigen::Vector3d delta = mergedSphere.center - spheres[i].center;
93  mergedSphere.radius = (delta.norm() + spheres[i].radius + mergedSphere.radius) / 2.0;
94  mergedSphere.center = delta.normalized() * (mergedSphere.radius - spheres[i].radius) + spheres[i].center;
95  }
96  }
97  }
98 }
99 
100 namespace bodies
101 {
102 template <typename T>
103 Body* constructBodyFromMsgHelper(const T& shape_msg, const geometry_msgs::Pose& pose)
104 {
105  shapes::Shape* shape = shapes::constructShapeFromMsg(shape_msg);
106 
107  if (shape)
108  {
109  Body* body = createBodyFromShape(shape);
110  if (body)
111  {
112  Eigen::Quaterniond q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
113  if (fabs(q.squaredNorm() - 1.0) > 1e-3)
114  {
115  CONSOLE_BRIDGE_logError("Quaternion is not normalized. Assuming identity.");
116  q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
117  }
118  Eigen::Affine3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) *
119  q.toRotationMatrix());
120  body->setPose(af);
121  return body;
122  }
123  }
124  return NULL;
125 }
126 }
127 
128 bodies::Body* bodies::constructBodyFromMsg(const shapes::ShapeMsg& shape_msg, const geometry_msgs::Pose& pose)
129 {
130  return constructBodyFromMsgHelper(shape_msg, pose);
131 }
132 
133 bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::Mesh& shape_msg, const geometry_msgs::Pose& pose)
134 {
135  return constructBodyFromMsgHelper(shape_msg, pose);
136 }
137 
138 bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::SolidPrimitive& shape_msg, const geometry_msgs::Pose& pose)
139 {
140  return constructBodyFromMsgHelper(shape_msg, pose);
141 }
142 
143 void bodies::computeBoundingSphere(const std::vector<const bodies::Body*>& bodies, bodies::BoundingSphere& sphere)
144 {
145  Eigen::Vector3d sum(0.0, 0.0, 0.0);
146 
147  // TODO - expand to all body types
148  unsigned int vertex_count = 0;
149  for (unsigned int i = 0; i < bodies.size(); i++)
150  {
151  const bodies::ConvexMesh* conv = dynamic_cast<const bodies::ConvexMesh*>(bodies[i]);
152  if (!conv)
153  continue;
154  for (unsigned int j = 0; j < conv->getScaledVertices().size(); j++, vertex_count++)
155  {
156  sum += conv->getPose() * conv->getScaledVertices()[j];
157  }
158  }
159 
160  sphere.center = sum / (double)vertex_count;
161 
162  double max_dist_squared = 0.0;
163  for (unsigned int i = 0; i < bodies.size(); i++)
164  {
165  const bodies::ConvexMesh* conv = dynamic_cast<const bodies::ConvexMesh*>(bodies[i]);
166  if (!conv)
167  continue;
168  for (unsigned int j = 0; j < conv->getScaledVertices().size(); j++)
169  {
170  double dist = (conv->getPose() * conv->getScaledVertices()[j] - sphere.center).squaredNorm();
171  if (dist > max_dist_squared)
172  {
173  max_dist_squared = dist;
174  }
175  }
176  }
177  sphere.radius = sqrt(max_dist_squared);
178 }
const Eigen::Affine3d & getPose() const
Retrieve the pose of the body.
Definition: bodies.h:142
d
void setPose(const Eigen::Affine3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:135
Body * constructBodyFromMsgHelper(const T &shape_msg, const geometry_msgs::Pose &pose)
Body * constructBodyFromMsg(const shape_msgs::Mesh &shape, const geometry_msgs::Pose &pose)
Create a body from a given shape.
Definition of a cylinder.
Definition: bodies.h:277
ShapeType type
The type of the shape.
Definition: shapes.h:102
Definition of a sphere.
Definition: bodies.h:228
Eigen::Vector3d center
Definition: bodies.h:61
Definition of a sphere that bounds another object.
Definition: bodies.h:59
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
Compute a bounding sphere to enclose a set of bounding spheres.
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:77
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:397
Definition of a box.
Definition: bodies.h:336
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
Shape * constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
Construct the shape that corresponds to the message. Return NULL on failure.
#define CONSOLE_BRIDGE_logError(fmt,...)
const EigenSTL::vector_Vector3d & getScaledVertices() const
Definition: bodies.cpp:994
void computeBoundingSphere(const std::vector< const Body * > &bodies, BoundingSphere &mergedSphere)
Compute the bounding sphere for a set of bodies and store the resulting sphere in mergedSphere...
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:88
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg
Type that can hold any of the desired shape message types.
This set of classes allows quickly detecting whether a given point is inside an object or not...
Definition: bodies.h:56


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Mon Jun 10 2019 13:22:04