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 <console_bridge/console.h>
41 #include <Eigen/Geometry>
42 
44 {
45  Body* body = nullptr;
46 
47  switch (shapeType)
48  {
49  case shapes::BOX:
50  body = new bodies::Box();
51  break;
52  case shapes::SPHERE:
53  body = new bodies::Sphere();
54  break;
55  case shapes::CYLINDER:
56  body = new bodies::Cylinder();
57  break;
58  case shapes::MESH:
59  body = new bodies::ConvexMesh();
60  break;
61  default:
62  CONSOLE_BRIDGE_logError("Creating body from shape: Unknown shape type %d", (int)shapeType);
63  break;
64  }
65  return body;
66 }
67 
69 {
70  Body* body = nullptr;
71 
72  if (shape)
73  {
74  body = createEmptyBodyFromShapeType(shape->type);
75  body->setDimensions(shape);
76  }
77 
78  return body;
79 }
80 
81 void bodies::mergeBoundingSpheres(const std::vector<BoundingSphere>& spheres, BoundingSphere& mergedSphere)
82 {
83  if (spheres.empty())
84  {
85  mergedSphere.center = Eigen::Vector3d(0.0, 0.0, 0.0);
86  mergedSphere.radius = 0.0;
87  }
88  else
89  {
90  mergedSphere = spheres[0];
91  for (unsigned int i = 1; i < spheres.size(); ++i)
92  {
93  if (spheres[i].radius <= 0.0)
94  continue;
95  Eigen::Vector3d diff = spheres[i].center - mergedSphere.center;
96  double d = diff.norm();
97  if (d + mergedSphere.radius <= spheres[i].radius)
98  {
99  mergedSphere.center = spheres[i].center;
100  mergedSphere.radius = spheres[i].radius;
101  }
102  else if (d + spheres[i].radius > mergedSphere.radius)
103  {
104  Eigen::Vector3d delta = mergedSphere.center - spheres[i].center;
105  mergedSphere.radius = (delta.norm() + spheres[i].radius + mergedSphere.radius) / 2.0;
106  mergedSphere.center = delta.normalized() * (mergedSphere.radius - spheres[i].radius) + spheres[i].center;
107  }
108  }
109  }
110 }
111 
112 namespace bodies
113 {
114 template <typename T>
115 Body* constructBodyFromMsgHelper(const T& shape_msg, const geometry_msgs::Pose& pose)
116 {
118 
119  if (shape)
120  {
121  Body* body = createEmptyBodyFromShapeType(shape->type);
122  if (body)
123  {
124  Eigen::Quaterniond q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
125  if (fabs(q.squaredNorm() - 1.0) > 1e-3)
126  {
127  CONSOLE_BRIDGE_logError("Quaternion is not normalized. Assuming identity.");
128  q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
129  }
130  Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q);
131  body->setPoseDirty(af);
132  body->setDimensionsDirty(shape.get());
133  body->updateInternalData();
134  return body;
135  }
136  }
137  return nullptr;
138 }
139 } // namespace bodies
140 
141 bodies::Body* bodies::constructBodyFromMsg(const shapes::ShapeMsg& shape_msg, const geometry_msgs::Pose& pose)
142 {
143  return constructBodyFromMsgHelper(shape_msg, pose);
144 }
145 
146 bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::Mesh& shape_msg, const geometry_msgs::Pose& pose)
147 {
148  return constructBodyFromMsgHelper(shape_msg, pose);
149 }
150 
151 bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::SolidPrimitive& shape_msg, const geometry_msgs::Pose& pose)
152 {
153  return constructBodyFromMsgHelper(shape_msg, pose);
154 }
155 
156 void bodies::computeBoundingSphere(const std::vector<const bodies::Body*>& bodies, bodies::BoundingSphere& sphere)
157 {
158  Eigen::Vector3d sum(0.0, 0.0, 0.0);
159 
160  // TODO - expand to all body types
161  unsigned int vertex_count = 0;
162  for (auto body : bodies)
163  {
164  if (!body || body->getType() != shapes::MESH)
165  continue;
166  // MESH type implies bodies::ConvexMesh
167  const bodies::ConvexMesh* conv = static_cast<const bodies::ConvexMesh*>(body);
168 
169  for (unsigned int j = 0; j < conv->getScaledVertices().size(); j++, vertex_count++)
170  {
171  sum += conv->getPose() * conv->getScaledVertices()[j];
172  }
173  }
174 
175  sphere.center = sum / (double)vertex_count;
176 
177  double max_dist_squared = 0.0;
178  for (auto body : bodies)
179  {
180  if (!body || body->getType() != shapes::MESH)
181  continue;
182  // MESH type implies bodies::ConvexMesh
183  const bodies::ConvexMesh* conv = static_cast<const bodies::ConvexMesh*>(body);
184 
185  for (unsigned int j = 0; j < conv->getScaledVertices().size(); j++)
186  {
187  double dist = (conv->getPose() * conv->getScaledVertices()[j] - sphere.center).squaredNorm();
188  if (dist > max_dist_squared)
189  {
190  max_dist_squared = dist;
191  }
192  }
193  }
194  sphere.radius = sqrt(max_dist_squared);
195 }
196 
197 void bodies::mergeBoundingBoxes(const std::vector<bodies::AABB>& boxes, bodies::AABB& mergedBox)
198 {
199  for (const auto& box : boxes)
200  mergedBox.extend(box);
201 }
d
Body * constructBodyFromMsgHelper(const T &shape_msg, const geometry_msgs::Pose &pose)
void setDimensionsDirty(const shapes::Shape *shape)
Set the dimensions of the body (from corresponding shape).
Definition: bodies.h:194
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...
Body * constructBodyFromMsg(const shape_msgs::Mesh &shape, const geometry_msgs::Pose &pose)
Create a body from a given shape.
Body * createEmptyBodyFromShapeType(const shapes::ShapeType &shapeType)
Create a body from a given shape.
Definition of a cylinder.
Definition: bodies.h:347
ShapeType type
The type of the shape.
Definition: shapes.h:102
Definition of a sphere.
Definition: bodies.h:285
const EigenSTL::vector_Vector3d & getScaledVertices() const
Definition: bodies.cpp:1099
std::shared_ptr< Shape > ShapePtr
Shared pointer to a Shape.
Definition: shapes.h:403
Eigen::Vector3d center
Definition: bodies.h:62
Definition of a sphere that bounds another object.
Definition: bodies.h:60
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:490
virtual void updateInternalData()=0
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition of a box.
Definition: bodies.h:416
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
ShapeType
A list of known shape types.
Definition: shapes.h:61
Shape * constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
Construct the shape that corresponds to the message. Return NULL on failure.
void setDimensions(const shapes::Shape *shape)
Set the dimensions of the body (from corresponding shape)
Definition: bodies.cpp:162
#define CONSOLE_BRIDGE_logError(fmt,...)
void setPoseDirty(const Eigen::Isometry3d &pose)
Set the pose of the body.
Definition: bodies.h:168
Represents an axis-aligned bounding box.
Definition: aabb.h:45
const Eigen::Isometry3d & getPose() const
Retrieve the pose of the body.
Definition: bodies.h:181
void mergeBoundingBoxes(const std::vector< AABB > &boxes, AABB &mergedBox)
Compute an axis-aligned bounding box to enclose a set of bounding boxes.
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: aabb.h:42


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Thu May 20 2021 02:12:07