body_operations.h
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 
35 /* Author: Ioan Sucan, E. Gil Jones */
36 
37 #ifndef GEOMETRIC_SHAPES_BODY_OPERATIONS_
38 #define GEOMETRIC_SHAPES_BODY_OPERATIONS_
39 
43 #include <geometry_msgs/Pose.h>
44 #include <visualization_msgs/Marker.h>
45 #include <vector>
46 
47 namespace bodies
48 {
50 Body* createEmptyBodyFromShapeType(const shapes::ShapeType& shapeType);
51 
53 Body* createBodyFromShape(const shapes::Shape* shape);
54 
56 Body* constructBodyFromMsg(const shape_msgs::Mesh& shape, const geometry_msgs::Pose& pose);
57 
59 Body* constructBodyFromMsg(const shape_msgs::SolidPrimitive& shape, const geometry_msgs::Pose& pose);
60 
62 Body* constructBodyFromMsg(const shapes::ShapeMsg& shape, const geometry_msgs::Pose& pose);
63 
66 
68 void constructMarkerFromBody(const bodies::Body* body, visualization_msgs::Marker& msg);
69 
71 void mergeBoundingSpheres(const std::vector<BoundingSphere>& spheres, BoundingSphere& mergedSphere);
72 
74 void mergeBoundingBoxes(const std::vector<AABB>& boxes, AABB& mergedBox);
75 
77 void mergeBoundingBoxesApprox(const std::vector<OBB>& boxes, OBB& mergedBox);
78 
80 void computeBoundingSphere(const std::vector<const Body*>& bodies, BoundingSphere& mergedSphere);
81 } // namespace bodies
82 #endif
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.
shapes::ShapeConstPtr constructShapeFromBody(const bodies::Body *body)
Get a shape that corresponds to this (scaled and padded) body.
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
Body * createBodyFromShape(const shapes::Shape *shape)
Create a body from a given shape.
ShapeType
A list of known shape types.
Definition: shapes.h:61
void constructMarkerFromBody(const bodies::Body *body, visualization_msgs::Marker &msg)
Construct a Marker message that corresponds to this (scaled and padded) body.
void mergeBoundingBoxesApprox(const std::vector< OBB > &boxes, OBB &mergedBox)
Compute an approximate oriented bounding box to enclose a set of bounding boxes.
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
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 Apr 14 2023 02:14:40