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 
36 #include <sstream>
37 #include <stdexcept>
38 
39 void geometric_shapes::constructMarkerFromShape(const shape_msgs::SolidPrimitive& shape_msg,
40  visualization_msgs::Marker& mk)
41 {
42  switch (shape_msg.type)
43  {
45  if (shape_msg.dimensions.size() <= shape_msgs::SolidPrimitive::SPHERE_RADIUS)
46  throw std::runtime_error("Insufficient dimensions in sphere definition");
47  else
48  {
50  mk.scale.x = mk.scale.y = mk.scale.z = shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] * 2.0;
51  }
52  break;
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");
58  else
59  {
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];
64  }
65  break;
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");
70  else
71  {
72  // there is no CONE marker, so this produces a cylinder marker as well
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];
77  }
78  break;
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");
83  else
84  {
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];
89  }
90  break;
91  default:
92  {
93  std::stringstream ss;
94  ss << shape_msg.type;
95  throw std::runtime_error("Unknown shape type: " + ss.str());
96  }
97  }
98 }
99 
100 void geometric_shapes::constructMarkerFromShape(const shape_msgs::Mesh& shape_msg, visualization_msgs::Marker& mk,
101  bool use_mesh_triangle_list)
102 {
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)
106  {
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)
110  {
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]]);
114  }
115  }
116  else
117  {
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)
121  {
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]]);
128  }
129  }
130 }
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.


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Mon Jun 10 2019 13:22:04