40 visualization_msgs::Marker& mk)
42 switch (shape_msg.type)
45 if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::SPHERE_RADIUS)
46 throw std::runtime_error(
"Insufficient dimensions in sphere definition");
50 mk.scale.x = mk.scale.y = mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] * 2.0;
54 if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_X ||
55 shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_Y ||
56 shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::BOX_Z)
57 throw std::runtime_error(
"Insufficient dimensions in box definition");
60 mk.type = visualization_msgs::Marker::CUBE;
61 mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X];
62 mk.scale.y = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y];
63 mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z];
67 if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CONE_RADIUS ||
68 shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CONE_HEIGHT)
69 throw std::runtime_error(
"Insufficient dimensions in cone definition");
74 mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] * 2.0;
75 mk.scale.y = mk.scale.x;
76 mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT];
80 if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CYLINDER_RADIUS ||
81 shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::CYLINDER_HEIGHT)
82 throw std::runtime_error(
"Insufficient dimensions in cylinder definition");
86 mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] * 2.0;
87 mk.scale.y = mk.scale.x;
88 mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT];
95 throw std::runtime_error(
"Unknown shape type: " + ss.str());
101 bool use_mesh_triangle_list)
103 if (shape_msg.triangles.empty() || shape_msg.vertices.empty())
104 throw std::runtime_error(
"Mesh definition is empty");
105 if (use_mesh_triangle_list)
107 mk.type = visualization_msgs::Marker::TRIANGLE_LIST;
108 mk.scale.x = mk.scale.y = mk.scale.z = 1.0;
109 for (std::size_t i = 0; i < shape_msg.triangles.size(); ++i)
111 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
112 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
113 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
118 mk.type = visualization_msgs::Marker::LINE_LIST;
119 mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
120 for (std::size_t i = 0; i < shape_msg.triangles.size(); ++i)
122 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
123 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
124 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
125 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
126 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
127 mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
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.