shape_to_marker.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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 
37 #include <sstream>
38 #include <stdexcept>
39 
40 void geometric_shapes::constructMarkerFromShape(const shape_msgs::SolidPrimitive& shape_msg,
41  visualization_msgs::Marker& mk)
42 {
43  switch (shape_msg.type)
44  {
46  if (shape_msg.dimensions.size() < geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::SPHERE>())
47  throw std::runtime_error("Insufficient dimensions in sphere definition");
48  else
49  {
51  mk.scale.x = mk.scale.y = mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] * 2.0;
52  }
53  break;
55  if (shape_msg.dimensions.size() < geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>())
56  throw std::runtime_error("Insufficient dimensions in box definition");
57  else
58  {
59  mk.type = visualization_msgs::Marker::CUBE;
60  mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X];
61  mk.scale.y = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y];
62  mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z];
63  }
64  break;
66  if (shape_msg.dimensions.size() < geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::CONE>())
67  throw std::runtime_error("Insufficient dimensions in cone definition");
68  else
69  {
70  // there is no CONE marker, so this produces a cylinder marker as well
72  mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] * 2.0;
73  mk.scale.y = mk.scale.x;
74  mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT];
75  }
76  break;
78  if (shape_msg.dimensions.size() <
79  geometric_shapes::solidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>())
80  throw std::runtime_error("Insufficient dimensions in cylinder definition");
81  else
82  {
84  mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] * 2.0;
85  mk.scale.y = mk.scale.x;
86  mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT];
87  }
88  break;
89  default:
90  {
91  std::stringstream ss;
92  ss << shape_msg.type;
93  throw std::runtime_error("Unknown shape type: " + ss.str());
94  }
95  }
96 }
97 
98 void geometric_shapes::constructMarkerFromShape(const shape_msgs::Mesh& shape_msg, visualization_msgs::Marker& mk,
99  bool use_mesh_triangle_list)
100 {
101  if (shape_msg.triangles.empty() || shape_msg.vertices.empty())
102  throw std::runtime_error("Mesh definition is empty");
103  if (use_mesh_triangle_list)
104  {
105  mk.type = visualization_msgs::Marker::TRIANGLE_LIST;
106  mk.scale.x = mk.scale.y = mk.scale.z = 1.0;
107  for (std::size_t i = 0; i < shape_msg.triangles.size(); ++i)
108  {
109  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
110  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
111  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
112  }
113  }
114  else
115  {
116  mk.type = visualization_msgs::Marker::LINE_LIST;
117  mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
118  for (std::size_t i = 0; i < shape_msg.triangles.size(); ++i)
119  {
120  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
121  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
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[2]]);
124  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
125  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
126  }
127  }
128 }
geometric_shapes::constructMarkerFromShape
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.
Definition: shape_to_marker.cpp:98
shape_to_marker.h
shapes::SPHERE
@ SPHERE
Definition: shapes.h:64
shapes::CONE
@ CONE
Definition: shapes.h:66
solid_primitive_dims.h
shapes::CYLINDER
@ CYLINDER
Definition: shapes.h:65
shapes::BOX
@ BOX
Definition: shapes.h:67


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Sun Oct 1 2023 02:40:16