shape_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 
35 /* Author: Ioan Sucan */
36 
38 
39 #include <cstdio>
40 #include <cmath>
41 #include <algorithm>
42 #include <set>
43 #include <float.h>
44 
46 
47 #include <Eigen/Geometry>
48 
52 
53 namespace shapes
54 {
55 Shape* constructShapeFromMsg(const shape_msgs::Plane& shape_msg)
56 {
57  return new Plane(shape_msg.coef[0], shape_msg.coef[1], shape_msg.coef[2], shape_msg.coef[3]);
58 }
59 
60 Shape* constructShapeFromMsg(const shape_msgs::Mesh& shape_msg)
61 {
62  if (shape_msg.triangles.empty() || shape_msg.vertices.empty())
63  {
64  CONSOLE_BRIDGE_logWarn("Mesh definition is empty");
65  return NULL;
66  }
67  else
68  {
69  EigenSTL::vector_Vector3d vertices(shape_msg.vertices.size());
70  std::vector<unsigned int> triangles(shape_msg.triangles.size() * 3);
71  for (unsigned int i = 0; i < shape_msg.vertices.size(); ++i)
72  vertices[i] = Eigen::Vector3d(shape_msg.vertices[i].x, shape_msg.vertices[i].y, shape_msg.vertices[i].z);
73  for (unsigned int i = 0; i < shape_msg.triangles.size(); ++i)
74  {
75  unsigned int i3 = i * 3;
76  triangles[i3++] = shape_msg.triangles[i].vertex_indices[0];
77  triangles[i3++] = shape_msg.triangles[i].vertex_indices[1];
78  triangles[i3] = shape_msg.triangles[i].vertex_indices[2];
79  }
80  return createMeshFromVertices(vertices, triangles);
81  }
82 }
83 
84 Shape* constructShapeFromMsg(const shape_msgs::SolidPrimitive& shape_msg)
85 {
86  Shape* shape = NULL;
87  if (shape_msg.type == shape_msgs::SolidPrimitive::SPHERE)
88  {
89  if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::SPHERE_RADIUS)
90  shape = new Sphere(shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS]);
91  }
92  else if (shape_msg.type == shape_msgs::SolidPrimitive::BOX)
93  {
94  if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_X &&
95  shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_Y &&
96  shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_Z)
97  shape = new Box(shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X],
98  shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y],
99  shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z]);
100  }
101  else if (shape_msg.type == shape_msgs::SolidPrimitive::CYLINDER)
102  {
103  if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CYLINDER_RADIUS &&
104  shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CYLINDER_HEIGHT)
105  shape = new Cylinder(shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS],
106  shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT]);
107  }
108  else if (shape_msg.type == shape_msgs::SolidPrimitive::CONE)
109  {
110  if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CONE_RADIUS &&
111  shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CONE_HEIGHT)
112  shape = new Cone(shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS],
113  shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT]);
114  }
115  if (shape == NULL)
116  CONSOLE_BRIDGE_logError("Unable to construct shape corresponding to shape_msg of type %d", (int)shape_msg.type);
117 
118  return shape;
119 }
120 
121 namespace
122 {
123 class ShapeVisitorAlloc : public boost::static_visitor<Shape*>
124 {
125 public:
126  Shape* operator()(const shape_msgs::Plane& shape_msg) const
127  {
128  return constructShapeFromMsg(shape_msg);
129  }
130 
131  Shape* operator()(const shape_msgs::Mesh& shape_msg) const
132  {
133  return constructShapeFromMsg(shape_msg);
134  }
135 
136  Shape* operator()(const shape_msgs::SolidPrimitive& shape_msg) const
137  {
138  return constructShapeFromMsg(shape_msg);
139  }
140 };
141 }
142 
144 {
145  return boost::apply_visitor(ShapeVisitorAlloc(), shape_msg);
146 }
147 
148 namespace
149 {
150 class ShapeVisitorMarker : public boost::static_visitor<void>
151 {
152 public:
153  ShapeVisitorMarker(visualization_msgs::Marker* marker, bool use_mesh_triangle_list)
154  : boost::static_visitor<void>(), use_mesh_triangle_list_(use_mesh_triangle_list), marker_(marker)
155  {
156  }
157 
158  void operator()(const shape_msgs::Plane& shape_msg) const
159  {
160  throw std::runtime_error("No visual markers can be constructed for planes");
161  }
162 
163  void operator()(const shape_msgs::Mesh& shape_msg) const
164  {
166  }
167 
168  void operator()(const shape_msgs::SolidPrimitive& shape_msg) const
169  {
171  }
172 
173 private:
175  visualization_msgs::Marker* marker_;
176 };
177 }
178 
179 bool constructMarkerFromShape(const Shape* shape, visualization_msgs::Marker& marker, bool use_mesh_triangle_list)
180 {
181  ShapeMsg shape_msg;
182  if (constructMsgFromShape(shape, shape_msg))
183  {
184  bool ok = false;
185  try
186  {
187  boost::apply_visitor(ShapeVisitorMarker(&marker, use_mesh_triangle_list), shape_msg);
188  ok = true;
189  }
190  catch (std::runtime_error& ex)
191  {
192  CONSOLE_BRIDGE_logError("%s", ex.what());
193  }
194  if (ok)
195  return true;
196  }
197  return false;
198 }
199 
200 namespace
201 {
202 class ShapeVisitorComputeExtents : public boost::static_visitor<Eigen::Vector3d>
203 {
204 public:
205  Eigen::Vector3d operator()(const shape_msgs::Plane& shape_msg) const
206  {
207  Eigen::Vector3d e(0.0, 0.0, 0.0);
208  return e;
209  }
210 
211  Eigen::Vector3d operator()(const shape_msgs::Mesh& shape_msg) const
212  {
213  double x_extent, y_extent, z_extent;
214  geometric_shapes::getShapeExtents(shape_msg, x_extent, y_extent, z_extent);
215  Eigen::Vector3d e(x_extent, y_extent, z_extent);
216  return e;
217  }
218 
219  Eigen::Vector3d operator()(const shape_msgs::SolidPrimitive& shape_msg) const
220  {
221  double x_extent, y_extent, z_extent;
222  geometric_shapes::getShapeExtents(shape_msg, x_extent, y_extent, z_extent);
223  Eigen::Vector3d e(x_extent, y_extent, z_extent);
224  return e;
225  }
226 };
227 }
228 
229 Eigen::Vector3d computeShapeExtents(const ShapeMsg& shape_msg)
230 {
231  return boost::apply_visitor(ShapeVisitorComputeExtents(), shape_msg);
232 }
233 
234 Eigen::Vector3d computeShapeExtents(const Shape* shape)
235 {
236  if (shape->type == SPHERE)
237  {
238  double d = static_cast<const Sphere*>(shape)->radius * 2.0;
239  return Eigen::Vector3d(d, d, d);
240  }
241  else if (shape->type == BOX)
242  {
243  const double* sz = static_cast<const Box*>(shape)->size;
244  return Eigen::Vector3d(sz[0], sz[1], sz[2]);
245  }
246  else if (shape->type == CYLINDER)
247  {
248  double d = static_cast<const Cylinder*>(shape)->radius * 2.0;
249  return Eigen::Vector3d(d, d, static_cast<const Cylinder*>(shape)->length);
250  }
251  else if (shape->type == CONE)
252  {
253  double d = static_cast<const Cone*>(shape)->radius * 2.0;
254  return Eigen::Vector3d(d, d, static_cast<const Cone*>(shape)->length);
255  }
256  else if (shape->type == MESH)
257  {
258  const Mesh* mesh = static_cast<const Mesh*>(shape);
259  if (mesh->vertex_count > 1)
260  {
261  std::vector<double> vmin(3, std::numeric_limits<double>::max());
262  std::vector<double> vmax(3, -std::numeric_limits<double>::max());
263  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
264  {
265  unsigned int i3 = i * 3;
266  for (unsigned int k = 0; k < 3; ++k)
267  {
268  unsigned int i3k = i3 + k;
269  if (mesh->vertices[i3k] > vmax[k])
270  vmax[k] = mesh->vertices[i3k];
271  if (mesh->vertices[i3k] < vmin[k])
272  vmin[k] = mesh->vertices[i3k];
273  }
274  }
275  return Eigen::Vector3d(vmax[0] - vmin[0], vmax[1] - vmin[1], vmax[2] - vmin[2]);
276  }
277  else
278  return Eigen::Vector3d(0.0, 0.0, 0.0);
279  }
280  else
281  return Eigen::Vector3d(0.0, 0.0, 0.0);
282 }
283 
284 void computeShapeBoundingSphere(const Shape* shape, Eigen::Vector3d& center, double& radius)
285 {
286  center.x() = 0.0;
287  center.y() = 0.0;
288  center.z() = 0.0;
289  radius = 0.0;
290 
291  if (shape->type == SPHERE)
292  {
293  radius = static_cast<const Sphere*>(shape)->radius;
294  }
295  else if (shape->type == BOX)
296  {
297  const double* sz = static_cast<const Box*>(shape)->size;
298  double half_width = sz[0] * 0.5;
299  double half_height = sz[1] * 0.5;
300  double half_depth = sz[2] * 0.5;
301  radius = std::sqrt(half_width * half_width + half_height * half_height + half_depth * half_depth);
302  }
303  else if (shape->type == CYLINDER)
304  {
305  double cyl_radius = static_cast<const Cylinder*>(shape)->radius;
306  double half_len = static_cast<const Cylinder*>(shape)->length * 0.5;
307  radius = std::sqrt(cyl_radius * cyl_radius + half_len * half_len);
308  }
309  else if (shape->type == CONE)
310  {
311  double cone_radius = static_cast<const Cone*>(shape)->radius;
312  double cone_height = static_cast<const Cone*>(shape)->length;
313 
314  if (cone_height > cone_radius)
315  {
316  // center of sphere is intersection of perpendicular bisectors:
317  double z = (cone_height - (cone_radius * cone_radius / cone_height)) * 0.5;
318  center.z() = z - (cone_height * 0.5);
319  radius = cone_height - z;
320  }
321  else
322  {
323  // short cone. Bounding sphere touches base only.
324  center.z() = -(cone_height * 0.5);
325  radius = cone_radius;
326  }
327  }
328  else if (shape->type == MESH)
329  {
330  const Mesh* mesh = static_cast<const Mesh*>(shape);
331  if (mesh->vertex_count > 1)
332  {
333  double mx = std::numeric_limits<double>::max();
334  Eigen::Vector3d min(mx, mx, mx);
335  Eigen::Vector3d max(-mx, -mx, -mx);
336  unsigned int cnt = mesh->vertex_count * 3;
337  for (unsigned int i = 0; i < cnt; i += 3)
338  {
339  Eigen::Vector3d v(mesh->vertices[i + 0], mesh->vertices[i + 1], mesh->vertices[i + 2]);
340  min = min.cwiseMin(v);
341  max = max.cwiseMax(v);
342  }
343 
344  center = (min + max) * 0.5;
345  radius = (max - min).norm() * 0.5;
346  }
347  }
348 }
349 
350 bool constructMsgFromShape(const Shape* shape, ShapeMsg& shape_msg)
351 {
352  if (shape->type == SPHERE)
353  {
354  shape_msgs::SolidPrimitive s;
357  s.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = static_cast<const Sphere*>(shape)->radius;
358  shape_msg = s;
359  }
360  else if (shape->type == BOX)
361  {
362  shape_msgs::SolidPrimitive s;
364  const double* sz = static_cast<const Box*>(shape)->size;
366  s.dimensions[shape_msgs::SolidPrimitive::BOX_X] = sz[0];
367  s.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = sz[1];
368  s.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = sz[2];
369  shape_msg = s;
370  }
371  else if (shape->type == CYLINDER)
372  {
373  shape_msgs::SolidPrimitive s;
376  s.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = static_cast<const Cylinder*>(shape)->radius;
377  s.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = static_cast<const Cylinder*>(shape)->length;
378  shape_msg = s;
379  }
380  else if (shape->type == CONE)
381  {
382  shape_msgs::SolidPrimitive s;
385  s.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] = static_cast<const Cone*>(shape)->radius;
386  s.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT] = static_cast<const Cone*>(shape)->length;
387  shape_msg = s;
388  }
389  else if (shape->type == PLANE)
390  {
391  shape_msgs::Plane s;
392  const Plane* p = static_cast<const Plane*>(shape);
393  s.coef[0] = p->a;
394  s.coef[1] = p->b;
395  s.coef[2] = p->c;
396  s.coef[3] = p->d;
397  shape_msg = s;
398  }
399  else if (shape->type == MESH)
400  {
401  shape_msgs::Mesh s;
402  const Mesh* mesh = static_cast<const Mesh*>(shape);
403  s.vertices.resize(mesh->vertex_count);
404  s.triangles.resize(mesh->triangle_count);
405 
406  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
407  {
408  unsigned int i3 = i * 3;
409  s.vertices[i].x = mesh->vertices[i3];
410  s.vertices[i].y = mesh->vertices[i3 + 1];
411  s.vertices[i].z = mesh->vertices[i3 + 2];
412  }
413 
414  for (unsigned int i = 0; i < s.triangles.size(); ++i)
415  {
416  unsigned int i3 = i * 3;
417  s.triangles[i].vertex_indices[0] = mesh->triangles[i3];
418  s.triangles[i].vertex_indices[1] = mesh->triangles[i3 + 1];
419  s.triangles[i].vertex_indices[2] = mesh->triangles[i3 + 2];
420  }
421  shape_msg = s;
422  }
423  else
424  {
425  CONSOLE_BRIDGE_logError("Unable to construct shape message for shape of type %d", (int)shape->type);
426  return false;
427  }
428 
429  return true;
430 }
431 
432 void saveAsText(const Shape* shape, std::ostream& out)
433 {
434  if (shape->type == SPHERE)
435  {
436  out << Sphere::STRING_NAME << std::endl;
437  out << static_cast<const Sphere*>(shape)->radius << std::endl;
438  }
439  else if (shape->type == BOX)
440  {
441  out << Box::STRING_NAME << std::endl;
442  const double* sz = static_cast<const Box*>(shape)->size;
443  out << sz[0] << " " << sz[1] << " " << sz[2] << std::endl;
444  }
445  else if (shape->type == CYLINDER)
446  {
447  out << Cylinder::STRING_NAME << std::endl;
448  out << static_cast<const Cylinder*>(shape)->radius << " " << static_cast<const Cylinder*>(shape)->length
449  << std::endl;
450  }
451  else if (shape->type == CONE)
452  {
453  out << Cone::STRING_NAME << std::endl;
454  out << static_cast<const Cone*>(shape)->radius << " " << static_cast<const Cone*>(shape)->length << std::endl;
455  }
456  else if (shape->type == PLANE)
457  {
458  out << Plane::STRING_NAME << std::endl;
459  const Plane* p = static_cast<const Plane*>(shape);
460  out << p->a << " " << p->b << " " << p->c << " " << p->d << std::endl;
461  }
462  else if (shape->type == MESH)
463  {
464  out << Mesh::STRING_NAME << std::endl;
465  const Mesh* mesh = static_cast<const Mesh*>(shape);
466  out << mesh->vertex_count << " " << mesh->triangle_count << std::endl;
467 
468  for (unsigned int i = 0; i < mesh->vertex_count; ++i)
469  {
470  unsigned int i3 = i * 3;
471  out << mesh->vertices[i3] << " " << mesh->vertices[i3 + 1] << " " << mesh->vertices[i3 + 2] << std::endl;
472  }
473 
474  for (unsigned int i = 0; i < mesh->triangle_count; ++i)
475  {
476  unsigned int i3 = i * 3;
477  out << mesh->triangles[i3] << " " << mesh->triangles[i3 + 1] << " " << mesh->triangles[i3 + 2] << std::endl;
478  }
479  }
480  else
481  {
482  CONSOLE_BRIDGE_logError("Unable to save shape of type %d", (int)shape->type);
483  }
484 }
485 
486 Shape* constructShapeFromText(std::istream& in)
487 {
488  Shape* result = NULL;
489  if (in.good() && !in.eof())
490  {
491  std::string type;
492  in >> type;
493  if (in.good() && !in.eof())
494  {
495  if (type == Sphere::STRING_NAME)
496  {
497  double radius;
498  in >> radius;
499  result = new Sphere(radius);
500  }
501  else if (type == Box::STRING_NAME)
502  {
503  double x, y, z;
504  in >> x >> y >> z;
505  result = new Box(x, y, z);
506  }
507  else if (type == Cylinder::STRING_NAME)
508  {
509  double r, l;
510  in >> r >> l;
511  result = new Cylinder(r, l);
512  }
513  else if (type == Cone::STRING_NAME)
514  {
515  double r, l;
516  in >> r >> l;
517  result = new Cone(r, l);
518  }
519  else if (type == Plane::STRING_NAME)
520  {
521  double a, b, c, d;
522  in >> a >> b >> c >> d;
523  result = new Plane(a, b, c, d);
524  }
525  else if (type == Mesh::STRING_NAME)
526  {
527  unsigned int v, t;
528  in >> v >> t;
529  Mesh* m = new Mesh(v, t);
530  result = m;
531  for (unsigned int i = 0; i < m->vertex_count; ++i)
532  {
533  unsigned int i3 = i * 3;
534  in >> m->vertices[i3] >> m->vertices[i3 + 1] >> m->vertices[i3 + 2];
535  }
536  for (unsigned int i = 0; i < m->triangle_count; ++i)
537  {
538  unsigned int i3 = i * 3;
539  in >> m->triangles[i3] >> m->triangles[i3 + 1] >> m->triangles[i3 + 2];
540  }
543  }
544  else
545  CONSOLE_BRIDGE_logError("Unknown shape type: '%s'", type.c_str());
546  }
547  }
548  return result;
549 }
550 
551 const std::string& shapeStringName(const Shape* shape)
552 {
553  static const std::string unknown = "unknown";
554  if (shape)
555  switch (shape->type)
556  {
557  case SPHERE:
558  return Sphere::STRING_NAME;
559  case CYLINDER:
560  return Cylinder::STRING_NAME;
561  case CONE:
562  return Cone::STRING_NAME;
563  case BOX:
564  return Box::STRING_NAME;
565  case PLANE:
566  return Plane::STRING_NAME;
567  case MESH:
568  return Mesh::STRING_NAME;
569  case OCTREE:
570  return OcTree::STRING_NAME;
571  default:
572  return unknown;
573  }
574  else
575  {
576  static const std::string empty;
577  return empty;
578  }
579 }
580 }
#define CONSOLE_BRIDGE_logWarn(fmt,...)
d
Definition of various shapes. No properties such as position are included. These are simply the descr...
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition: shapes.h:127
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
double b
Definition: shapes.h:260
ShapeType type
The type of the shape.
Definition: shapes.h:102
static const std::string STRING_NAME
The type of the shape, as a string.
Definition: shapes.h:136
double d
Definition: shapes.h:260
double c
Definition: shapes.h:260
unsigned int * triangles
The vertex indices for each triangle triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1...
Definition: shapes.h:233
double y
Definition of a plane with equation ax + by + cz + d = 0.
Definition: shapes.h:245
double a
The plane equation is ax + by + cz + d = 0.
Definition: shapes.h:260
visualization_msgs::Marker * marker_
Definition of a cone Tip is on positive z axis. Center of base is on negative z axis. Origin is halway between tip and center of base.
Definition: shapes.h:152
unsigned int vertex_count
The number of available vertices.
Definition: shapes.h:222
Mesh * createMeshFromVertices(const EigenSTL::vector_Vector3d &vertices, const std::vector< unsigned int > &triangles)
Load a mesh from a set of vertices.
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:77
unsigned int triangle_count
The number of triangles formed with the vertices.
Definition: shapes.h:229
double z
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
Definition: shapes.h:226
void saveAsText(const Shape *shape, std::ostream &out)
Save all the information about this shape as plain text.
void computeVertexNormals()
Compute vertex normals by averaging from adjacent triangle normals.
Definition: shapes.cpp:444
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.
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
const std::string & shapeStringName(const Shape *shape)
Get the string name of the shape.
void computeTriangleNormals()
Compute the normals of each triangle from its vertices via cross product.
Definition: shapes.cpp:421
void computeShapeBoundingSphere(const Shape *shape, Eigen::Vector3d &center, double &radius)
Compute a sphere bounding a shape.
#define CONSOLE_BRIDGE_logError(fmt,...)
static const std::string STRING_NAME
The type of the shape, as a string.
Definition: shapes.h:252
static const std::string STRING_NAME
The type of the shape, as a string.
Definition: shapes.h:271
bool constructMsgFromShape(const Shape *shape, ShapeMsg &shape_msg)
Construct the message that corresponds to the shape. Return false on failure.
bool use_mesh_triangle_list_
Definition of a triangle mesh By convention the "center" of the shape is at the origin. For a mesh this implies that the AABB of the mesh is centered at the origin. Some methods may not work with arbitrary meshes whose AABB is not centered at the origin.
Definition: shapes.h:196
Definition of a box Aligned with the XYZ axes.
Definition: shapes.h:174
static const std::string STRING_NAME
The type of the shape, as a string.
Definition: shapes.h:204
void constructMarkerFromShape(const shape_msgs::Mesh &shape_msg, visualization_msgs::Marker &marker, bool use_mesh_triangle_list=true)
Convert a shape_msgs::Mesh shape_msg to a visualization_msgs::Marker marker.
Shape * constructShapeFromText(std::istream &in)
Construct a shape from plain text description.
void getShapeExtents(const shape_msgs::SolidPrimitive &shape_msg, double &x_extent, double &y_extent, double &z_extent)
Get the dimensions of an axis-aligned bounding box for the shape described by shape_msg.
static const std::string STRING_NAME
The type of the shape, as a string.
Definition: shapes.h:115
static const std::string STRING_NAME
The type of the shape, as a string.
Definition: shapes.h:181
Eigen::Vector3d computeShapeExtents(const ShapeMsg &shape_msg)
Compute the extents of a shape.
double x
r
static const std::string STRING_NAME
The type of the shape, as a string.
Definition: shapes.h:159
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg
Type that can hold any of the desired shape message types.
The number of dimensions of a particular shape.


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Mon Jun 10 2019 13:22:04