Go to the documentation of this file.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 #include <shape_tools/shape_to_marker.h>
00036 #include <sstream>
00037 #include <stdexcept>
00038
00039 void shape_tools::constructMarkerFromShape(const shape_msgs::SolidPrimitive &shape_msg, visualization_msgs::Marker &mk)
00040 {
00041 switch (shape_msg.type)
00042 {
00043 case shape_msgs::SolidPrimitive::SPHERE:
00044 if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::SPHERE_RADIUS)
00045 throw std::runtime_error("Insufficient dimensions in sphere definition");
00046 else
00047 {
00048 mk.type = visualization_msgs::Marker::SPHERE;
00049 mk.scale.x = mk.scale.y = mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] * 2.0;
00050 }
00051 break;
00052 case shape_msgs::SolidPrimitive::BOX:
00053 if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_X ||
00054 shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_Y ||
00055 shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_Z)
00056 throw std::runtime_error("Insufficient dimensions in box definition");
00057 else
00058 {
00059 mk.type = visualization_msgs::Marker::CUBE;
00060 mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X];
00061 mk.scale.y = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y];
00062 mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z];
00063 }
00064 break;
00065 case shape_msgs::SolidPrimitive::CONE:
00066 if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CONE_RADIUS ||
00067 shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CONE_HEIGHT)
00068 throw std::runtime_error("Insufficient dimensions in cone definition");
00069 else
00070 {
00071
00072 mk.type = visualization_msgs::Marker::CYLINDER;
00073 mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] * 2.0;
00074 mk.scale.y = mk.scale.x;
00075 mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT];
00076 }
00077 break;
00078 case shape_msgs::SolidPrimitive::CYLINDER:
00079 if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CYLINDER_RADIUS ||
00080 shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CYLINDER_HEIGHT)
00081 throw std::runtime_error("Insufficient dimensions in cylinder definition");
00082 else
00083 {
00084 mk.type = visualization_msgs::Marker::CYLINDER;
00085 mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] * 2.0;
00086 mk.scale.y = mk.scale.x;
00087 mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT];
00088 }
00089 break;
00090 default:
00091 {
00092 std::stringstream ss;
00093 ss << shape_msg.type;
00094 throw std::runtime_error("Unknown shape type: " + ss.str());
00095 }
00096 }
00097 }
00098
00099 void shape_tools::constructMarkerFromShape(const shape_msgs::Mesh &shape_msg, visualization_msgs::Marker &mk, bool use_mesh_triangle_list)
00100 {
00101 if (shape_msg.triangles.empty() || shape_msg.vertices.empty())
00102 throw std::runtime_error("Mesh definition is empty");
00103 if (use_mesh_triangle_list)
00104 {
00105 mk.type = visualization_msgs::Marker::TRIANGLE_LIST;
00106 mk.scale.x = mk.scale.y = mk.scale.z = 1.0;
00107 for (std::size_t i = 0 ; i < shape_msg.triangles.size() ; ++i)
00108 {
00109 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
00110 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
00111 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
00112 }
00113 }
00114 else
00115 {
00116 mk.type = visualization_msgs::Marker::LINE_LIST;
00117 mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
00118 for (std::size_t i = 0 ; i < shape_msg.triangles.size() ; ++i)
00119 {
00120 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
00121 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
00122 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
00123 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
00124 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
00125 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
00126 }
00127 }
00128
00129 }