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


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Sun Aug 22 2021 02:42:25