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() <
48  throw std::runtime_error("Insufficient dimensions in sphere definition");
49  else
50  {
52  mk.scale.x = mk.scale.y = mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] * 2.0;
53  }
54  break;
56  if (shape_msg.dimensions.size() <
58  throw std::runtime_error("Insufficient dimensions in box definition");
59  else
60  {
61  mk.type = visualization_msgs::Marker::CUBE;
62  mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X];
63  mk.scale.y = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y];
64  mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z];
65  }
66  break;
68  if (shape_msg.dimensions.size() <
70  throw std::runtime_error("Insufficient dimensions in cone definition");
71  else
72  {
73  // there is no CONE marker, so this produces a cylinder marker as well
75  mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] * 2.0;
76  mk.scale.y = mk.scale.x;
77  mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT];
78  }
79  break;
81  if (shape_msg.dimensions.size() <
83  throw std::runtime_error("Insufficient dimensions in cylinder definition");
84  else
85  {
87  mk.scale.x = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] * 2.0;
88  mk.scale.y = mk.scale.x;
89  mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT];
90  }
91  break;
92  default:
93  {
94  std::stringstream ss;
95  ss << shape_msg.type;
96  throw std::runtime_error("Unknown shape type: " + ss.str());
97  }
98  }
99 }
100 
101 void geometric_shapes::constructMarkerFromShape(const shape_msgs::Mesh& shape_msg, visualization_msgs::Marker& mk,
102  bool use_mesh_triangle_list)
103 {
104  if (shape_msg.triangles.empty() || shape_msg.vertices.empty())
105  throw std::runtime_error("Mesh definition is empty");
106  if (use_mesh_triangle_list)
107  {
108  mk.type = visualization_msgs::Marker::TRIANGLE_LIST;
109  mk.scale.x = mk.scale.y = mk.scale.z = 1.0;
110  for (std::size_t i = 0; i < shape_msg.triangles.size(); ++i)
111  {
112  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
113  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
114  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
115  }
116  }
117  else
118  {
119  mk.type = visualization_msgs::Marker::LINE_LIST;
120  mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
121  for (std::size_t i = 0; i < shape_msg.triangles.size(); ++i)
122  {
123  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[0]]);
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[0]]);
126  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
127  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[1]]);
128  mk.points.push_back(shape_msg.vertices[shape_msg.triangles[i].vertex_indices[2]]);
129  }
130  }
131 }
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.
The number of dimensions of a particular shape.


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Sun May 2 2021 02:13:07