00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "geometric_shapes/shape_operations.h"
00038
00039 #include <cstdio>
00040 #include <cmath>
00041 #include <algorithm>
00042 #include <set>
00043 #include <float.h>
00044
00045 #include <console_bridge/console.h>
00046
00047 #include <Eigen/Geometry>
00048
00049 #include <geometric_shapes/shape_to_marker.h>
00050 #include <geometric_shapes/shape_extents.h>
00051 #include <geometric_shapes/solid_primitive_dims.h>
00052
00053 namespace shapes
00054 {
00055
00056 Shape* constructShapeFromMsg(const shape_msgs::Plane &shape_msg)
00057 {
00058 return new Plane(shape_msg.coef[0], shape_msg.coef[1], shape_msg.coef[2], shape_msg.coef[3]);
00059 }
00060
00061 Shape* constructShapeFromMsg(const shape_msgs::Mesh &shape_msg)
00062 {
00063 if (shape_msg.triangles.empty() || shape_msg.vertices.empty())
00064 {
00065 logWarn("Mesh definition is empty");
00066 return NULL;
00067 }
00068 else
00069 {
00070 EigenSTL::vector_Vector3d vertices(shape_msg.vertices.size());
00071 std::vector<unsigned int> triangles(shape_msg.triangles.size() * 3);
00072 for (unsigned int i = 0 ; i < shape_msg.vertices.size() ; ++i)
00073 vertices[i] = Eigen::Vector3d(shape_msg.vertices[i].x, shape_msg.vertices[i].y, shape_msg.vertices[i].z);
00074 for (unsigned int i = 0 ; i < shape_msg.triangles.size() ; ++i)
00075 {
00076 unsigned int i3 = i * 3;
00077 triangles[i3++] = shape_msg.triangles[i].vertex_indices[0];
00078 triangles[i3++] = shape_msg.triangles[i].vertex_indices[1];
00079 triangles[i3] = shape_msg.triangles[i].vertex_indices[2];
00080 }
00081 return createMeshFromVertices(vertices, triangles);
00082 }
00083 }
00084
00085 Shape* constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
00086 {
00087 Shape *shape = NULL;
00088 if (shape_msg.type == shape_msgs::SolidPrimitive::SPHERE)
00089 {
00090 if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::SPHERE_RADIUS)
00091 shape = new Sphere(shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS]);
00092 }
00093 else
00094 if (shape_msg.type == shape_msgs::SolidPrimitive::BOX)
00095 {
00096 if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_X &&
00097 shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_Y &&
00098 shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_Z)
00099 shape = new Box(shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X],
00100 shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y],
00101 shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z]);
00102 }
00103 else
00104 if (shape_msg.type == shape_msgs::SolidPrimitive::CYLINDER)
00105 {
00106 if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CYLINDER_RADIUS &&
00107 shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CYLINDER_HEIGHT)
00108 shape = new Cylinder(shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS],
00109 shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT]);
00110 }
00111 else
00112 if (shape_msg.type == shape_msgs::SolidPrimitive::CONE)
00113 {
00114 if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CONE_RADIUS &&
00115 shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CONE_HEIGHT)
00116 shape = new Cone(shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS],
00117 shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT]);
00118 }
00119 if (shape == NULL)
00120 logError("Unable to construct shape corresponding to shape_msg of type %d", (int)shape_msg.type);
00121
00122 return shape;
00123 }
00124
00125 namespace
00126 {
00127
00128 class ShapeVisitorAlloc : public boost::static_visitor<Shape*>
00129 {
00130 public:
00131
00132 Shape* operator()(const shape_msgs::Plane &shape_msg) const
00133 {
00134 return constructShapeFromMsg(shape_msg);
00135 }
00136
00137 Shape* operator()(const shape_msgs::Mesh &shape_msg) const
00138 {
00139 return constructShapeFromMsg(shape_msg);
00140 }
00141
00142 Shape* operator()(const shape_msgs::SolidPrimitive &shape_msg) const
00143 {
00144 return constructShapeFromMsg(shape_msg);
00145 }
00146 };
00147
00148 }
00149
00150 Shape* constructShapeFromMsg(const ShapeMsg &shape_msg)
00151 {
00152 return boost::apply_visitor(ShapeVisitorAlloc(), shape_msg);
00153 }
00154
00155 namespace
00156 {
00157
00158 class ShapeVisitorMarker : public boost::static_visitor<void>
00159 {
00160 public:
00161
00162 ShapeVisitorMarker(visualization_msgs::Marker *marker, bool use_mesh_triangle_list) :
00163 boost::static_visitor<void>(),
00164 use_mesh_triangle_list_(use_mesh_triangle_list),
00165 marker_(marker)
00166 {
00167 }
00168
00169 void operator()(const shape_msgs::Plane &shape_msg) const
00170 {
00171 throw std::runtime_error("No visual markers can be constructed for planes");
00172 }
00173
00174 void operator()(const shape_msgs::Mesh &shape_msg) const
00175 {
00176 geometric_shapes::constructMarkerFromShape(shape_msg, *marker_, use_mesh_triangle_list_);
00177 }
00178
00179 void operator()(const shape_msgs::SolidPrimitive &shape_msg) const
00180 {
00181 geometric_shapes::constructMarkerFromShape(shape_msg, *marker_);
00182 }
00183
00184 private:
00185
00186 bool use_mesh_triangle_list_;
00187 visualization_msgs::Marker *marker_;
00188 };
00189
00190 }
00191
00192 bool constructMarkerFromShape(const Shape* shape, visualization_msgs::Marker &marker, bool use_mesh_triangle_list)
00193 {
00194 ShapeMsg shape_msg;
00195 if (constructMsgFromShape(shape, shape_msg))
00196 {
00197 bool ok = false;
00198 try
00199 {
00200 boost::apply_visitor(ShapeVisitorMarker(&marker, use_mesh_triangle_list), shape_msg);
00201 ok = true;
00202 }
00203 catch (std::runtime_error &ex)
00204 {
00205 logError("%s", ex.what());
00206 }
00207 if (ok)
00208 return true;
00209 }
00210 return false;
00211 }
00212
00213 namespace
00214 {
00215
00216 class ShapeVisitorComputeExtents : public boost::static_visitor<Eigen::Vector3d>
00217 {
00218 public:
00219
00220 Eigen::Vector3d operator()(const shape_msgs::Plane &shape_msg) const
00221 {
00222 Eigen::Vector3d e(0.0, 0.0, 0.0);
00223 return e;
00224 }
00225
00226 Eigen::Vector3d operator()(const shape_msgs::Mesh &shape_msg) const
00227 {
00228 double x_extent, y_extent, z_extent;
00229 geometric_shapes::getShapeExtents(shape_msg, x_extent, y_extent, z_extent);
00230 Eigen::Vector3d e(x_extent, y_extent, z_extent);
00231 return e;
00232 }
00233
00234 Eigen::Vector3d operator()(const shape_msgs::SolidPrimitive &shape_msg) const
00235 {
00236 double x_extent, y_extent, z_extent;
00237 geometric_shapes::getShapeExtents(shape_msg, x_extent, y_extent, z_extent);
00238 Eigen::Vector3d e(x_extent, y_extent, z_extent);
00239 return e;
00240 }
00241 };
00242
00243 }
00244
00245 Eigen::Vector3d computeShapeExtents(const ShapeMsg &shape_msg)
00246 {
00247 return boost::apply_visitor(ShapeVisitorComputeExtents(), shape_msg);
00248 }
00249
00250 Eigen::Vector3d computeShapeExtents(const Shape *shape)
00251 {
00252 if (shape->type == SPHERE)
00253 {
00254 double d = static_cast<const Sphere*>(shape)->radius * 2.0;
00255 return Eigen::Vector3d(d, d, d);
00256 }
00257 else
00258 if (shape->type == BOX)
00259 {
00260 const double* sz = static_cast<const Box*>(shape)->size;
00261 return Eigen::Vector3d(sz[0], sz[1], sz[2]);
00262 }
00263 else
00264 if (shape->type == CYLINDER)
00265 {
00266 double d = static_cast<const Cylinder*>(shape)->radius * 2.0;
00267 return Eigen::Vector3d(d, d, static_cast<const Cylinder*>(shape)->length);
00268 }
00269 else
00270 if (shape->type == CONE)
00271 {
00272 double d = static_cast<const Cone*>(shape)->radius * 2.0;
00273 return Eigen::Vector3d(d, d, static_cast<const Cone*>(shape)->length);
00274 }
00275 else
00276 if (shape->type == MESH)
00277 {
00278 const Mesh *mesh = static_cast<const Mesh*>(shape);
00279 if (mesh->vertex_count > 1)
00280 {
00281 std::vector<double> vmin(3, std::numeric_limits<double>::max());
00282 std::vector<double> vmax(3, -std::numeric_limits<double>::max());
00283 for (unsigned int i = 0; i < mesh->vertex_count ; ++i)
00284 {
00285 unsigned int i3 = i * 3;
00286 for (unsigned int k = 0 ; k < 3 ; ++k)
00287 {
00288 unsigned int i3k = i3 + k;
00289 if (mesh->vertices[i3k] > vmax[k])
00290 vmax[k] = mesh->vertices[i3k];
00291 if (mesh->vertices[i3k] < vmin[k])
00292 vmin[k] = mesh->vertices[i3k];
00293 }
00294 }
00295 return Eigen::Vector3d(vmax[0] - vmin[0], vmax[1] - vmin[1], vmax[2] - vmin[2]);
00296 }
00297 else
00298 return Eigen::Vector3d(0.0, 0.0, 0.0);
00299 }
00300 else
00301 return Eigen::Vector3d(0.0, 0.0, 0.0);
00302 }
00303
00304 void computeShapeBoundingSphere(const Shape *shape, Eigen::Vector3d& center, double& radius)
00305 {
00306 center.x() = 0.0;
00307 center.y() = 0.0;
00308 center.z() = 0.0;
00309 radius = 0.0;
00310
00311 if (shape->type == SPHERE)
00312 {
00313 radius = static_cast<const Sphere*>(shape)->radius;
00314 }
00315 else if (shape->type == BOX)
00316 {
00317 const double* sz = static_cast<const Box*>(shape)->size;
00318 double half_width = sz[0] * 0.5;
00319 double half_height = sz[1] * 0.5;
00320 double half_depth = sz[2] * 0.5;
00321 radius = std::sqrt( half_width * half_width +
00322 half_height * half_height +
00323 half_depth * half_depth);
00324 }
00325 else if (shape->type == CYLINDER)
00326 {
00327 double cyl_radius = static_cast<const Cylinder*>(shape)->radius;
00328 double half_len = static_cast<const Cylinder*>(shape)->length * 0.5;
00329 radius = std::sqrt( cyl_radius * cyl_radius +
00330 half_len * half_len);
00331 }
00332 else if (shape->type == CONE)
00333 {
00334 double cone_radius = static_cast<const Cone*>(shape)->radius;
00335 double cone_height = static_cast<const Cone*>(shape)->length;
00336
00337 if (cone_height > cone_radius)
00338 {
00339
00340 double z = (cone_height - (cone_radius * cone_radius / cone_height)) * 0.5;
00341 center.z() = z - (cone_height * 0.5);
00342 radius = cone_height - z;
00343 }
00344 else
00345 {
00346
00347 center.z() = - (cone_height * 0.5);
00348 radius = cone_radius;
00349 }
00350 }
00351 else if (shape->type == MESH)
00352 {
00353 const Mesh *mesh = static_cast<const Mesh*>(shape);
00354 if (mesh->vertex_count > 1)
00355 {
00356 double mx = std::numeric_limits<double>::max();
00357 Eigen::Vector3d min( mx, mx, mx);
00358 Eigen::Vector3d max(-mx, -mx, -mx);
00359 unsigned int cnt = mesh->vertex_count * 3;
00360 for (unsigned int i = 0; i < cnt ; i+=3)
00361 {
00362 Eigen::Vector3d v(mesh->vertices[i+0], mesh->vertices[i+1], mesh->vertices[i+2]);
00363 min = min.cwiseMin(v);
00364 max = max.cwiseMax(v);
00365 }
00366
00367 center = (min + max) * 0.5;
00368 radius = (max - min).norm() * 0.5;
00369 }
00370 }
00371 }
00372
00373
00374 bool constructMsgFromShape(const Shape* shape, ShapeMsg &shape_msg)
00375 {
00376 if (shape->type == SPHERE)
00377 {
00378 shape_msgs::SolidPrimitive s;
00379 s.type = shape_msgs::SolidPrimitive::SPHERE;
00380 s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::SPHERE>::value);
00381 s.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = static_cast<const Sphere*>(shape)->radius;
00382 shape_msg = s;
00383 }
00384 else
00385 if (shape->type == BOX)
00386 {
00387 shape_msgs::SolidPrimitive s;
00388 s.type = shape_msgs::SolidPrimitive::BOX;
00389 const double* sz = static_cast<const Box*>(shape)->size;
00390 s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
00391 s.dimensions[shape_msgs::SolidPrimitive::BOX_X] = sz[0];
00392 s.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = sz[1];
00393 s.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = sz[2];
00394 shape_msg = s;
00395 }
00396 else
00397 if (shape->type == CYLINDER)
00398 {
00399 shape_msgs::SolidPrimitive s;
00400 s.type = shape_msgs::SolidPrimitive::CYLINDER;
00401 s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>::value);
00402 s.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = static_cast<const Cylinder*>(shape)->radius;
00403 s.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = static_cast<const Cylinder*>(shape)->length;
00404 shape_msg = s;
00405 }
00406 else
00407 if (shape->type == CONE)
00408 {
00409 shape_msgs::SolidPrimitive s;
00410 s.type = shape_msgs::SolidPrimitive::CONE;
00411 s.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CONE>::value);
00412 s.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] = static_cast<const Cone*>(shape)->radius;
00413 s.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT] = static_cast<const Cone*>(shape)->length;
00414 shape_msg = s;
00415 }
00416 else
00417 if (shape->type == PLANE)
00418 {
00419 shape_msgs::Plane s;
00420 const Plane *p = static_cast<const Plane*>(shape);
00421 s.coef[0] = p->a;
00422 s.coef[1] = p->b;
00423 s.coef[2] = p->c;
00424 s.coef[3] = p->d;
00425 shape_msg = s;
00426 }
00427 else
00428 if (shape->type == MESH)
00429 {
00430 shape_msgs::Mesh s;
00431 const Mesh *mesh = static_cast<const Mesh*>(shape);
00432 s.vertices.resize(mesh->vertex_count);
00433 s.triangles.resize(mesh->triangle_count);
00434
00435 for (unsigned int i = 0 ; i < mesh->vertex_count ; ++i)
00436 {
00437 unsigned int i3 = i * 3;
00438 s.vertices[i].x = mesh->vertices[i3];
00439 s.vertices[i].y = mesh->vertices[i3 + 1];
00440 s.vertices[i].z = mesh->vertices[i3 + 2];
00441 }
00442
00443 for (unsigned int i = 0 ; i < s.triangles.size() ; ++i)
00444 {
00445 unsigned int i3 = i * 3;
00446 s.triangles[i].vertex_indices[0] = mesh->triangles[i3];
00447 s.triangles[i].vertex_indices[1] = mesh->triangles[i3 + 1];
00448 s.triangles[i].vertex_indices[2] = mesh->triangles[i3 + 2];
00449 }
00450 shape_msg = s;
00451 }
00452 else
00453 {
00454 logError("Unable to construct shape message for shape of type %d", (int)shape->type);
00455 return false;
00456 }
00457
00458 return true;
00459 }
00460
00461 void saveAsText(const Shape *shape, std::ostream &out)
00462 {
00463 if (shape->type == SPHERE)
00464 {
00465 out << Sphere::STRING_NAME << std::endl;
00466 out << static_cast<const Sphere*>(shape)->radius << std::endl;
00467 }
00468 else
00469 if (shape->type == BOX)
00470 {
00471 out << Box::STRING_NAME << std::endl;
00472 const double* sz = static_cast<const Box*>(shape)->size;
00473 out << sz[0] << " " << sz[1] << " " << sz[2] << std::endl;
00474 }
00475 else
00476 if (shape->type == CYLINDER)
00477 {
00478 out << Cylinder::STRING_NAME << std::endl;
00479 out << static_cast<const Cylinder*>(shape)->radius << " " << static_cast<const Cylinder*>(shape)->length << std::endl;
00480 }
00481 else
00482 if (shape->type == CONE)
00483 {
00484 out << Cone::STRING_NAME << std::endl;
00485 out << static_cast<const Cone*>(shape)->radius << " " << static_cast<const Cone*>(shape)->length << std::endl;
00486 }
00487 else
00488 if (shape->type == PLANE)
00489 {
00490 out << Plane::STRING_NAME << std::endl;
00491 const Plane *p = static_cast<const Plane*>(shape);
00492 out << p->a << " " << p->b << " " << p->c << " " << p->d << std::endl;
00493 }
00494 else
00495 if (shape->type == MESH)
00496 {
00497 out << Mesh::STRING_NAME << std::endl;
00498 const Mesh *mesh = static_cast<const Mesh*>(shape);
00499 out << mesh->vertex_count << " " << mesh->triangle_count << std::endl;
00500
00501 for (unsigned int i = 0 ; i < mesh->vertex_count ; ++i)
00502 {
00503 unsigned int i3 = i * 3;
00504 out << mesh->vertices[i3] << " " << mesh->vertices[i3 + 1] << " " << mesh->vertices[i3 + 2] << std::endl;
00505 }
00506
00507 for (unsigned int i = 0 ; i < mesh->triangle_count ; ++i)
00508 {
00509 unsigned int i3 = i * 3;
00510 out << mesh->triangles[i3] << " " << mesh->triangles[i3 + 1] << " " << mesh->triangles[i3 + 2] << std::endl;
00511 }
00512 }
00513 else
00514 {
00515 logError("Unable to save shape of type %d", (int)shape->type);
00516 }
00517 }
00518
00519 Shape* constructShapeFromText(std::istream &in)
00520 {
00521 Shape *result = NULL;
00522 if (in.good() && !in.eof())
00523 {
00524 std::string type;
00525 in >> type;
00526 if (in.good() && !in.eof())
00527 {
00528 if (type == Sphere::STRING_NAME)
00529 {
00530 double radius;
00531 in >> radius;
00532 result = new Sphere(radius);
00533 }
00534 else
00535 if (type == Box::STRING_NAME)
00536 {
00537 double x, y, z;
00538 in >> x >> y >> z;
00539 result = new Box(x, y, z);
00540 }
00541 else
00542 if (type == Cylinder::STRING_NAME)
00543 {
00544 double r, l;
00545 in >> r >> l;
00546 result = new Cylinder(r, l);
00547 }
00548 else
00549 if (type == Cone::STRING_NAME)
00550 {
00551 double r, l;
00552 in >> r >> l;
00553 result = new Cone(r, l);
00554 }
00555 else
00556 if (type == Plane::STRING_NAME)
00557 {
00558 double a, b, c, d;
00559 in >> a >> b >> c >> d;
00560 result = new Plane(a, b, c, d);
00561 }
00562 else
00563 if (type == Mesh::STRING_NAME)
00564 {
00565 unsigned int v, t;
00566 in >> v >> t;
00567 Mesh *m = new Mesh(v, t);
00568 result = m;
00569 for (unsigned int i = 0 ; i < m->vertex_count ; ++i)
00570 {
00571 unsigned int i3 = i * 3;
00572 in >> m->vertices[i3] >> m->vertices[i3 + 1] >> m->vertices[i3 + 2];
00573 }
00574 for (unsigned int i = 0 ; i < m->triangle_count ; ++i)
00575 {
00576 unsigned int i3 = i * 3;
00577 in >> m->triangles[i3] >> m->triangles[i3 + 1] >> m->triangles[i3 + 2];
00578 }
00579 m->computeTriangleNormals();
00580 m->computeVertexNormals();
00581 }
00582 else
00583 logError("Unknown shape type: '%s'", type.c_str());
00584 }
00585 }
00586 return result;
00587 }
00588
00589 const std::string& shapeStringName(const Shape *shape)
00590 {
00591 static const std::string unknown = "unknown";
00592 if (shape)
00593 switch (shape->type)
00594 {
00595 case SPHERE:
00596 return Sphere::STRING_NAME;
00597 case CYLINDER:
00598 return Cylinder::STRING_NAME;
00599 case CONE:
00600 return Cone::STRING_NAME;
00601 case BOX:
00602 return Box::STRING_NAME;
00603 case PLANE:
00604 return Plane::STRING_NAME;
00605 case MESH:
00606 return Mesh::STRING_NAME;
00607 case OCTREE:
00608 return OcTree::STRING_NAME;
00609 default:
00610 return unknown;
00611 }
00612 else
00613 {
00614 static const std::string empty;
00615 return empty;
00616 }
00617 }
00618
00619 }