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 
39 #include <console_bridge/console.h>
40 #include <Eigen/Geometry>
41 
43 {
44  Body* body = nullptr;
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::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q);
119  body->setPose(af);
120  return body;
121  }
122  }
123  return nullptr;
124 }
125 } // namespace bodies
126 
127 bodies::Body* bodies::constructBodyFromMsg(const shapes::ShapeMsg& shape_msg, const geometry_msgs::Pose& pose)
128 {
129  return constructBodyFromMsgHelper(shape_msg, pose);
130 }
131 
132 bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::Mesh& shape_msg, const geometry_msgs::Pose& pose)
133 {
134  return constructBodyFromMsgHelper(shape_msg, pose);
135 }
136 
137 bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::SolidPrimitive& shape_msg, const geometry_msgs::Pose& pose)
138 {
139  return constructBodyFromMsgHelper(shape_msg, pose);
140 }
141 
142 void bodies::computeBoundingSphere(const std::vector<const bodies::Body*>& bodies, bodies::BoundingSphere& sphere)
143 {
144  Eigen::Vector3d sum(0.0, 0.0, 0.0);
145 
146  // TODO - expand to all body types
147  unsigned int vertex_count = 0;
148  for (auto body : bodies)
149  {
150  const bodies::ConvexMesh* conv = dynamic_cast<const bodies::ConvexMesh*>(body);
151  if (!conv)
152  continue;
153  for (unsigned int j = 0; j < conv->getScaledVertices().size(); j++, vertex_count++)
154  {
155  sum += conv->getPose() * conv->getScaledVertices()[j];
156  }
157  }
158 
159  sphere.center = sum / (double)vertex_count;
160 
161  double max_dist_squared = 0.0;
162  for (auto body : bodies)
163  {
164  const bodies::ConvexMesh* conv = dynamic_cast<const bodies::ConvexMesh*>(body);
165  if (!conv)
166  continue;
167  for (unsigned int j = 0; j < conv->getScaledVertices().size(); j++)
168  {
169  double dist = (conv->getPose() * conv->getScaledVertices()[j] - sphere.center).squaredNorm();
170  if (dist > max_dist_squared)
171  {
172  max_dist_squared = dist;
173  }
174  }
175  }
176  sphere.radius = sqrt(max_dist_squared);
177 }
178 
179 void bodies::mergeBoundingBoxes(const std::vector<bodies::AABB>& boxes, bodies::AABB& mergedBox)
180 {
181  for (const auto& box : boxes)
182  mergedBox.extend(box);
183 }
d
Body * constructBodyFromMsgHelper(const T &shape_msg, const geometry_msgs::Pose &pose)
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:136
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.
Definition of a cylinder.
Definition: bodies.h:283
ShapeType type
The type of the shape.
Definition: shapes.h:102
Definition of a sphere.
Definition: bodies.h:233
const EigenSTL::vector_Vector3d & getScaledVertices() const
Definition: bodies.cpp:1027
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:405
Definition of a box.
Definition: bodies.h:343
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,...)
Represents an axis-aligned bounding box.
Definition: aabb.h:45
const Eigen::Isometry3d & getPose() const
Retrieve the pose of the body.
Definition: bodies.h:143
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:89
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 Wed Nov 20 2019 03:16:42