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 
82 {
83  shapes::ShapePtr result;
84 
85  switch (body->getType())
86  {
87  case shapes::SPHERE:
88  {
89  // As we already know body's type, we can skip the vtable lookup and use compile-time polymorphism
90  const auto& dims = static_cast<const bodies::Sphere*>(body)->bodies::Sphere::getScaledDimensions();
91  result.reset(new shapes::Sphere(dims[0]));
92  break;
93  }
94  case shapes::BOX:
95  {
96  const auto& dims = static_cast<const bodies::Box*>(body)->bodies::Box::getScaledDimensions();
97  result.reset(new shapes::Box(dims[0], dims[1], dims[2]));
98  break;
99  }
100  case shapes::CYLINDER:
101  {
102  const auto& dims = static_cast<const bodies::Cylinder*>(body)->bodies::Cylinder::getScaledDimensions();
103  result.reset(new shapes::Cylinder(dims[0], dims[1]));
104  break;
105  }
106  case shapes::MESH:
107  {
108  const auto mesh = static_cast<const bodies::ConvexMesh*>(body);
109  const auto& scaledVertices = mesh->getScaledVertices();
110 
111  // createMeshFromVertices requires an "expanded" list of triangles where each triangle is
112  // represented by its three vertex positions
113  EigenSTL::vector_Vector3d vertexList;
114  vertexList.reserve(3 * mesh->getTriangles().size());
115  for (const auto& triangle : mesh->getTriangles())
116  vertexList.push_back(scaledVertices[triangle]);
117 
118  result.reset(shapes::createMeshFromVertices(vertexList));
119  break;
120  }
121  default:
122  {
123  CONSOLE_BRIDGE_logError("Unknown body type: %d", (int)body->getType());
124  break;
125  }
126  }
127  return result;
128 }
129 
130 void bodies::constructMarkerFromBody(const bodies::Body* body, visualization_msgs::Marker& msg)
131 {
132  auto shape = bodies::constructShapeFromBody(body);
133  shapes::constructMarkerFromShape(shape.get(), msg, true);
134  const auto& pose = body->getPose();
135  msg.pose.position.x = pose.translation().x();
136  msg.pose.position.y = pose.translation().y();
137  msg.pose.position.z = pose.translation().z();
138 
139  ASSERT_ISOMETRY(pose);
140  Eigen::Quaterniond quat(pose.linear().matrix());
141  msg.pose.orientation.x = quat.x();
142  msg.pose.orientation.y = quat.y();
143  msg.pose.orientation.z = quat.z();
144  msg.pose.orientation.w = quat.w();
145 }
146 
147 void bodies::mergeBoundingSpheres(const std::vector<BoundingSphere>& spheres, BoundingSphere& mergedSphere)
148 {
149  if (spheres.empty())
150  {
151  mergedSphere.center = Eigen::Vector3d(0.0, 0.0, 0.0);
152  mergedSphere.radius = 0.0;
153  }
154  else
155  {
156  mergedSphere = spheres[0];
157  for (unsigned int i = 1; i < spheres.size(); ++i)
158  {
159  if (spheres[i].radius <= 0.0)
160  continue;
161  Eigen::Vector3d diff = spheres[i].center - mergedSphere.center;
162  double d = diff.norm();
163  if (d + mergedSphere.radius <= spheres[i].radius)
164  {
165  mergedSphere.center = spheres[i].center;
166  mergedSphere.radius = spheres[i].radius;
167  }
168  else if (d + spheres[i].radius > mergedSphere.radius)
169  {
170  Eigen::Vector3d delta = mergedSphere.center - spheres[i].center;
171  mergedSphere.radius = (delta.norm() + spheres[i].radius + mergedSphere.radius) / 2.0;
172  mergedSphere.center = delta.normalized() * (mergedSphere.radius - spheres[i].radius) + spheres[i].center;
173  }
174  }
175  }
176 }
177 
178 namespace bodies
179 {
180 template <typename T>
181 Body* constructBodyFromMsgHelper(const T& shape_msg, const geometry_msgs::Pose& pose)
182 {
184 
185  if (shape)
186  {
187  Body* body = createEmptyBodyFromShapeType(shape->type);
188  if (body)
189  {
190  Eigen::Quaterniond q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
191  if (fabs(q.squaredNorm() - 1.0) > 1e-3)
192  {
193  CONSOLE_BRIDGE_logError("Quaternion is not normalized. Assuming identity.");
194  q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
195  }
196  Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q);
197  body->setPoseDirty(af);
198  body->setDimensionsDirty(shape.get());
199  body->updateInternalData();
200  return body;
201  }
202  }
203  return nullptr;
204 }
205 } // namespace bodies
206 
207 bodies::Body* bodies::constructBodyFromMsg(const shapes::ShapeMsg& shape_msg, const geometry_msgs::Pose& pose)
208 {
209  return constructBodyFromMsgHelper(shape_msg, pose);
210 }
211 
212 bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::Mesh& shape_msg, const geometry_msgs::Pose& pose)
213 {
214  return constructBodyFromMsgHelper(shape_msg, pose);
215 }
216 
217 bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::SolidPrimitive& shape_msg, const geometry_msgs::Pose& pose)
218 {
219  return constructBodyFromMsgHelper(shape_msg, pose);
220 }
221 
222 void bodies::computeBoundingSphere(const std::vector<const bodies::Body*>& bodies, bodies::BoundingSphere& sphere)
223 {
224  Eigen::Vector3d sum(0.0, 0.0, 0.0);
225 
226  // TODO - expand to all body types
227  unsigned int vertex_count = 0;
228  for (auto body : bodies)
229  {
230  if (!body || body->getType() != shapes::MESH)
231  continue;
232  // MESH type implies bodies::ConvexMesh
233  const bodies::ConvexMesh* conv = static_cast<const bodies::ConvexMesh*>(body);
234 
235  for (unsigned int j = 0; j < conv->getScaledVertices().size(); j++, vertex_count++)
236  {
237  sum += conv->getPose() * conv->getScaledVertices()[j];
238  }
239  }
240 
241  sphere.center = sum / (double)vertex_count;
242 
243  double max_dist_squared = 0.0;
244  for (auto body : bodies)
245  {
246  if (!body || body->getType() != shapes::MESH)
247  continue;
248  // MESH type implies bodies::ConvexMesh
249  const bodies::ConvexMesh* conv = static_cast<const bodies::ConvexMesh*>(body);
250 
251  for (unsigned int j = 0; j < conv->getScaledVertices().size(); j++)
252  {
253  double dist = (conv->getPose() * conv->getScaledVertices()[j] - sphere.center).squaredNorm();
254  if (dist > max_dist_squared)
255  {
256  max_dist_squared = dist;
257  }
258  }
259  }
260  sphere.radius = sqrt(max_dist_squared);
261 }
262 
263 void bodies::mergeBoundingBoxes(const std::vector<bodies::AABB>& boxes, bodies::AABB& mergedBox)
264 {
265  for (const auto& box : boxes)
266  mergedBox.extend(box);
267 }
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...
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
std::vector< double > getScaledDimensions() const override
Get the dimensions associated to this body (scaled and padded)
Definition: bodies.cpp:199
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
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:351
shapes::ShapeConstPtr constructShapeFromBody(const bodies::Body *body)
Get a shape that corresponds to this (scaled and padded) body.
ShapeType type
The type of the shape.
Definition: shapes.h:102
Definition of a sphere.
Definition: bodies.h:288
const EigenSTL::vector_Vector3d & getScaledVertices() const
Definition: bodies.cpp:1111
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
std::vector< double > getScaledDimensions() const override
Get the dimensions associated to this body (scaled and padded)
Definition: bodies.cpp:597
Mesh * createMeshFromVertices(const EigenSTL::vector_Vector3d &vertices, const std::vector< unsigned int > &triangles)
Load a mesh from a set of vertices.
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:496
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:421
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
#define ASSERT_ISOMETRY(transform)
Assert that the given transform is an isometry.
ShapeType
A list of known shape types.
Definition: shapes.h:61
bool constructMarkerFromShape(const Shape *shape, visualization_msgs::Marker &mk, bool use_mesh_triangle_list=false)
Construct the marker that corresponds to the shape. Return false on failure.
void constructMarkerFromBody(const bodies::Body *body, visualization_msgs::Marker &msg)
Construct a Marker message that corresponds to this (scaled and padded) body.
Shape * constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
Construct the shape that corresponds to the message. Return NULL on failure.
Definition of a sphere.
Definition: shapes.h:106
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:226
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.
shapes::ShapeType getType() const
Get the type of shape this body represents.
Definition: bodies.h:101
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.
std::vector< double > getScaledDimensions() const override
Get the dimensions associated to this body (scaled and padded)
Definition: bodies.cpp:369
This set of classes allows quickly detecting whether a given point is inside an object or not...
Definition: aabb.h:42
std::shared_ptr< const Shape > ShapeConstPtr
Shared pointer to a const Shape.
Definition: shapes.h:406


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Fri Aug 26 2022 02:12:34