47 #include <Eigen/Geometry> 57 return new Plane(shape_msg.coef[0], shape_msg.coef[1], shape_msg.coef[2], shape_msg.coef[3]);
62 if (shape_msg.triangles.empty() || shape_msg.vertices.empty())
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)
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];
89 if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::SPHERE_RADIUS)
90 shape =
new Sphere(shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS]);
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]);
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]);
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]);
116 CONSOLE_BRIDGE_logError(
"Unable to construct shape corresponding to shape_msg of type %d", (
int)shape_msg.type);
123 class ShapeVisitorAlloc :
public boost::static_visitor<Shape*>
126 Shape* operator()(
const shape_msgs::Plane& shape_msg)
const 131 Shape* operator()(
const shape_msgs::Mesh& shape_msg)
const 136 Shape* operator()(
const shape_msgs::SolidPrimitive& shape_msg)
const 145 return boost::apply_visitor(ShapeVisitorAlloc(), shape_msg);
150 class ShapeVisitorMarker :
public boost::static_visitor<void>
153 ShapeVisitorMarker(visualization_msgs::Marker* marker,
bool use_mesh_triangle_list)
158 void operator()(
const shape_msgs::Plane& shape_msg)
const 160 throw std::runtime_error(
"No visual markers can be constructed for planes");
163 void operator()(
const shape_msgs::Mesh& shape_msg)
const 168 void operator()(
const shape_msgs::SolidPrimitive& shape_msg)
const 187 boost::apply_visitor(ShapeVisitorMarker(&marker, use_mesh_triangle_list), shape_msg);
190 catch (std::runtime_error& ex)
202 class ShapeVisitorComputeExtents :
public boost::static_visitor<Eigen::Vector3d>
205 Eigen::Vector3d operator()(
const shape_msgs::Plane& shape_msg)
const 207 Eigen::Vector3d e(0.0, 0.0, 0.0);
211 Eigen::Vector3d operator()(
const shape_msgs::Mesh& shape_msg)
const 213 double x_extent, y_extent, z_extent;
215 Eigen::Vector3d e(x_extent, y_extent, z_extent);
219 Eigen::Vector3d operator()(
const shape_msgs::SolidPrimitive& shape_msg)
const 221 double x_extent, y_extent, z_extent;
223 Eigen::Vector3d e(x_extent, y_extent, z_extent);
231 return boost::apply_visitor(ShapeVisitorComputeExtents(), shape_msg);
238 double d =
static_cast<const Sphere*
>(shape)->radius * 2.0;
239 return Eigen::Vector3d(d, d, d);
243 const double* sz =
static_cast<const Box*
>(shape)->size;
244 return Eigen::Vector3d(sz[0], sz[1], sz[2]);
248 double d =
static_cast<const Cylinder*
>(shape)->radius * 2.0;
249 return Eigen::Vector3d(d, d, static_cast<const Cylinder*>(shape)->length);
253 double d =
static_cast<const Cone*
>(shape)->radius * 2.0;
254 return Eigen::Vector3d(d, d, static_cast<const Cone*>(shape)->length);
258 const Mesh* mesh =
static_cast<const Mesh*
>(shape);
261 std::vector<double> vmin(3, std::numeric_limits<double>::max());
262 std::vector<double> vmax(3, -std::numeric_limits<double>::max());
265 unsigned int i3 = i * 3;
266 for (
unsigned int k = 0; k < 3; ++k)
268 unsigned int i3k = i3 + k;
275 return Eigen::Vector3d(vmax[0] - vmin[0], vmax[1] - vmin[1], vmax[2] - vmin[2]);
278 return Eigen::Vector3d(0.0, 0.0, 0.0);
281 return Eigen::Vector3d(0.0, 0.0, 0.0);
293 radius =
static_cast<const Sphere*
>(shape)->radius;
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);
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);
311 double cone_radius =
static_cast<const Cone*
>(shape)->radius;
312 double cone_height =
static_cast<const Cone*
>(shape)->length;
314 if (cone_height > cone_radius)
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;
324 center.z() = -(cone_height * 0.5);
325 radius = cone_radius;
330 const Mesh* mesh =
static_cast<const Mesh*
>(shape);
333 double mx = std::numeric_limits<double>::max();
334 Eigen::Vector3d min(mx, mx, mx);
335 Eigen::Vector3d max(-mx, -mx, -mx);
337 for (
unsigned int i = 0; i < cnt; i += 3)
340 min = min.cwiseMin(v);
341 max = max.cwiseMax(v);
344 center = (min + max) * 0.5;
345 radius = (max - min).norm() * 0.5;
354 shape_msgs::SolidPrimitive s;
357 s.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] =
static_cast<const Sphere*
>(shape)->radius;
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];
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;
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;
392 const Plane* p =
static_cast<const Plane*
>(shape);
402 const Mesh* mesh =
static_cast<const Mesh*
>(shape);
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];
414 for (
unsigned int i = 0; i < s.triangles.size(); ++i)
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];
437 out << static_cast<const Sphere*>(shape)->radius << std::endl;
442 const double* sz =
static_cast<const Box*
>(shape)->size;
443 out << sz[0] <<
" " << sz[1] <<
" " << sz[2] << std::endl;
448 out << static_cast<const Cylinder*>(shape)->radius <<
" " << static_cast<const Cylinder*>(shape)->length
454 out << static_cast<const Cone*>(shape)->radius <<
" " << static_cast<const Cone*>(shape)->length << std::endl;
459 const Plane* p =
static_cast<const Plane*
>(shape);
460 out << p->
a <<
" " << p->
b <<
" " << p->
c <<
" " << p->
d << std::endl;
465 const Mesh* mesh =
static_cast<const Mesh*
>(shape);
470 unsigned int i3 = i * 3;
476 unsigned int i3 = i * 3;
488 Shape* result = NULL;
489 if (in.good() && !in.eof())
493 if (in.good() && !in.eof())
499 result =
new Sphere(radius);
505 result =
new Box(x, y, z);
517 result =
new Cone(r, l);
522 in >> a >> b >> c >> d;
523 result =
new Plane(a, b, c, d);
533 unsigned int i3 = i * 3;
538 unsigned int i3 = i * 3;
553 static const std::string unknown =
"unknown";
576 static const std::string empty;
#define CONSOLE_BRIDGE_logWarn(fmt,...)
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.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
ShapeType type
The type of the shape.
static const std::string STRING_NAME
The type of the shape, as a string.
unsigned int * triangles
The vertex indices for each triangle triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1...
Definition of a plane with equation ax + by + cz + d = 0.
double a
The plane equation is ax + by + cz + d = 0.
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.
unsigned int vertex_count
The number of available vertices.
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.
unsigned int triangle_count
The number of triangles formed with the vertices.
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
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.
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.
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.
void computeShapeBoundingSphere(const Shape *shape, Eigen::Vector3d ¢er, 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.
static const std::string STRING_NAME
The type of the shape, as a string.
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 of a box Aligned with the XYZ axes.
static const std::string STRING_NAME
The type of the shape, as a string.
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.
static const std::string STRING_NAME
The type of the shape, as a string.
Eigen::Vector3d computeShapeExtents(const ShapeMsg &shape_msg)
Compute the extents of a shape.
static const std::string STRING_NAME
The type of the shape, as a string.
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.