marker_creation.cpp
Go to the documentation of this file.
2 #include <urdf_model/link.h>
3 #include <tf2_eigen/tf2_eigen.h>
4 #include <ros/console.h>
5 
6 namespace vm = visualization_msgs;
7 
8 namespace rviz_marker_tools {
9 
10 std_msgs::ColorRGBA& setColor(std_msgs::ColorRGBA& color, Color color_id, double alpha) {
11  switch (color_id) {
12  case RED:
13  color.r = 0.8;
14  color.g = 0.1;
15  color.b = 0.1;
16  color.a = alpha;
17  break;
18  case GREEN:
19  color.r = 0.1;
20  color.g = 0.8;
21  color.b = 0.1;
22  color.a = alpha;
23  break;
24  case BLUE:
25  color.r = 0.1;
26  color.g = 0.1;
27  color.b = 0.8;
28  color.a = alpha;
29  break;
30  case WHITE:
31  color.r = 1.0;
32  color.g = 1.0;
33  color.b = 1.0;
34  color.a = alpha;
35  break;
36  case GREY:
37  color.r = 0.9;
38  color.g = 0.9;
39  color.b = 0.9;
40  color.a = alpha;
41  break;
42  case DARK_GREY:
43  color.r = 0.6;
44  color.g = 0.6;
45  color.b = 0.6;
46  color.a = alpha;
47  break;
48  case BLACK:
49  color.r = 0.0;
50  color.g = 0.0;
51  color.b = 0.0;
52  color.a = alpha;
53  break;
54  case YELLOW:
55  color.r = 1.0;
56  color.g = 1.0;
57  color.b = 0.0;
58  color.a = alpha;
59  break;
60  case ORANGE:
61  color.r = 1.0;
62  color.g = 0.5;
63  color.b = 0.0;
64  color.a = alpha;
65  break;
66  case BROWN:
67  color.r = 0.597;
68  color.g = 0.296;
69  color.b = 0.0;
70  color.a = alpha;
71  break;
72  case PINK:
73  color.r = 1.0;
74  color.g = 0.4;
75  color.b = 1;
76  color.a = alpha;
77  break;
78  case LIME_GREEN:
79  color.r = 0.6;
80  color.g = 1.0;
81  color.b = 0.2;
82  color.a = alpha;
83  break;
84  case PURPLE:
85  color.r = 0.597;
86  color.g = 0.0;
87  color.b = 0.597;
88  color.a = alpha;
89  break;
90  case CYAN:
91  color.r = 0.0;
92  color.g = 1.0;
93  color.b = 1.0;
94  color.a = alpha;
95  break;
96  case MAGENTA:
97  color.r = 1.0;
98  color.g = 0.0;
99  color.b = 1.0;
100  color.a = alpha;
101  break;
102  }
103  return color;
104 }
105 
106 // interpolate between start and end with fraction in range from 0..1
107 double interpolate(double start, double end, double fraction) {
108  return start * (1.0 - fraction) + end * fraction;
109 }
110 
111 std_msgs::ColorRGBA& interpolate(std_msgs::ColorRGBA& color, const std_msgs::ColorRGBA& other, double fraction) {
112  if (fraction < 0.0)
113  fraction = 0.0;
114  if (fraction > 1.0)
115  fraction = 1.0;
116  color.r = interpolate(color.r, other.r, fraction);
117  color.g = interpolate(color.g, other.g, fraction);
118  color.b = interpolate(color.b, other.b, fraction);
119  color.a = interpolate(color.a, other.a, fraction);
120  return color;
121 }
122 
123 std_msgs::ColorRGBA& brighten(std_msgs::ColorRGBA& color, double fraction) {
124  static std_msgs::ColorRGBA white;
125  if (white.r == 0.0)
126  setColor(white, WHITE);
127  return interpolate(color, white, fraction);
128 }
129 
130 std_msgs::ColorRGBA& darken(std_msgs::ColorRGBA& color, double fraction) {
131  static std_msgs::ColorRGBA black;
132  return interpolate(color, black, fraction);
133 }
134 
135 std_msgs::ColorRGBA getColor(Color color, double alpha) {
136  std_msgs::ColorRGBA result;
137  setColor(result, color, alpha);
138  return result;
139 }
140 
141 geometry_msgs::Pose composePoses(const geometry_msgs::Pose& first, const Eigen::Isometry3d& second) {
142  Eigen::Isometry3d result_eigen;
143  tf2::fromMsg(first, result_eigen);
144  result_eigen = result_eigen * second;
145  return tf2::toMsg(result_eigen);
146 }
147 
148 geometry_msgs::Pose composePoses(const Eigen::Isometry3d& first, const geometry_msgs::Pose& second) {
149  Eigen::Isometry3d result_eigen;
150  tf2::fromMsg(second, result_eigen);
151  result_eigen = first * result_eigen;
152  return tf2::toMsg(result_eigen);
153 }
154 
155 void prepareMarker(vm::Marker& m, int marker_type) {
156  m.action = vm::Marker::ADD;
157  m.type = marker_type;
158  m.points.clear();
159  m.colors.clear();
160 
161  // ensure non-null orientation
162  if (m.pose.orientation.w == 0 && m.pose.orientation.x == 0 && m.pose.orientation.y == 0 && m.pose.orientation.z == 0)
163  m.pose.orientation.w = 1.0;
164 }
165 
166 vm::Marker& makeXYPlane(vm::Marker& m) {
167  geometry_msgs::Point p[4];
168 
169  p[0].x = 1.0;
170  p[0].y = 1.0;
171  p[0].z = 0.0;
172 
173  p[1].x = -1.0;
174  p[1].y = 1.0;
175  p[1].z = 0.0;
176 
177  p[2].x = -1.0;
178  p[2].y = -1.0;
179  p[2].z = 0.0;
180 
181  p[3].x = 1.0;
182  p[3].y = -1.0;
183  p[3].z = 0.0;
184 
185  m.scale.x = m.scale.y = m.scale.z = 1.0;
186  prepareMarker(m, vm::Marker::TRIANGLE_LIST);
187  m.points.push_back(p[0]);
188  m.points.push_back(p[1]);
189  m.points.push_back(p[2]);
190 
191  m.points.push_back(p[2]);
192  m.points.push_back(p[3]);
193  m.points.push_back(p[0]);
194  return m;
195 }
196 
197 vm::Marker& makeXZPlane(vm::Marker& m) {
198  makeXYPlane(m);
199  // swap y and z components of points
200  for (auto& p : m.points)
201  std::swap(p.y, p.z);
202  return m;
203 }
204 
205 vm::Marker& makeYZPlane(vm::Marker& m) {
206  makeXZPlane(m);
207  // (additionally) swap x and y components of points
208  for (auto& p : m.points)
209  std::swap(p.x, p.y);
210  return m;
211 }
212 
214 vm::Marker makeCone(double angle, vm::Marker& m) {
215  m.scale.x = m.scale.y = m.scale.z = 1.0;
216  prepareMarker(m, vm::Marker::TRIANGLE_LIST);
217  geometry_msgs::Point p[3];
218  p[0].x = p[0].y = p[0].z = 0.0;
219  p[1].x = p[2].x = 1.0;
220 
221  const double delta_theta = M_PI / 16.0;
222  double theta = 0;
223 
224  for (std::size_t i = 0; i < 32; i++) {
225  p[1].y = cos(theta) / angle;
226  p[1].z = sin(theta) / angle;
227 
228  p[2].y = cos(theta + delta_theta) / angle;
229  p[2].z = sin(theta + delta_theta) / angle;
230 
231  m.points.push_back(p[0]);
232  m.points.push_back(p[1]);
233  m.points.push_back(p[2]);
234 
235  theta += delta_theta;
236  }
237  return m;
238 }
239 
240 vm::Marker& makeSphere(vm::Marker& m, double radius) {
241  m.scale.x = m.scale.y = m.scale.z = radius;
242  prepareMarker(m, vm::Marker::SPHERE);
243  return m;
244 }
245 
246 vm::Marker& makeBox(vm::Marker& m, double x, double y, double z) {
247  m.scale.x = x;
248  m.scale.y = y;
249  m.scale.z = z;
250  prepareMarker(m, vm::Marker::CUBE);
251  return m;
252 }
253 
254 vm::Marker& makeCylinder(vm::Marker& m, double diameter, double height) {
255  m.scale.x = m.scale.y = diameter;
256  m.scale.z = height;
257  prepareMarker(m, vm::Marker::CYLINDER);
258  return m;
259 }
260 
261 vm::Marker& makeMesh(vm::Marker& m, const std::string& filename, double sx, double sy, double sz) {
262  m.scale.x = sx;
263  m.scale.y = sy;
264  m.scale.z = sz;
265  prepareMarker(m, vm::Marker::MESH_RESOURCE);
266  m.mesh_resource = filename;
267  m.mesh_use_embedded_materials = 1u;
268  return m;
269 }
270 
271 vm::Marker& makeArrow(vm::Marker& m, const Eigen::Vector3d& start_point, const Eigen::Vector3d& end_point,
272  double diameter, double head_length) {
273  // scale.y is set according to default proportions in rviz/default_plugin/markers/arrow_marker.cpp#L61
274  // for the default head_length=0, the head length will keep the default proportion defined in arrow_marker.cpp#L106
275  m.scale.x = diameter;
276  m.scale.y = 2 * diameter;
277  m.scale.z = head_length;
278  prepareMarker(m, vm::Marker::ARROW);
279  m.points.resize(2);
280  m.points[0] = tf2::toMsg(start_point);
281  m.points[1] = tf2::toMsg(end_point);
282  return m;
283 }
284 
285 vm::Marker& makeArrow(vm::Marker& m, double scale, bool tip_at_origin) {
286  m.scale.y = m.scale.z = 0.1 * scale;
287  m.scale.x = scale;
288  prepareMarker(m, vm::Marker::ARROW);
289  if (tip_at_origin)
290  m.pose = composePoses(m.pose, Eigen::Translation3d(-scale, 0, 0) * Eigen::Isometry3d::Identity());
291  return m;
292 }
293 
294 vm::Marker& makeText(vm::Marker& m, const std::string& text) {
295  m.scale.x = m.scale.y = m.scale.z = 1.0;
296  prepareMarker(m, vm::Marker::TEXT_VIEW_FACING);
297  m.text = text;
298  return m;
299 }
300 
301 vm::Marker& makeFromGeometry(vm::Marker& m, const urdf::Geometry& geom) {
302  switch (geom.type) {
303  case urdf::Geometry::SPHERE: {
304  const urdf::Sphere& sphere = static_cast<const urdf::Sphere&>(geom);
305  makeSphere(m, sphere.radius);
306  break;
307  }
308  case urdf::Geometry::BOX: {
309  const urdf::Box& box = static_cast<const urdf::Box&>(geom);
310  makeBox(m, box.dim.x, box.dim.y, box.dim.z);
311  break;
312  }
313  case urdf::Geometry::CYLINDER: {
314  const urdf::Cylinder& cylinder = static_cast<const urdf::Cylinder&>(geom);
315  makeCylinder(m, 2.0 * cylinder.radius, cylinder.length);
316  break;
317  }
318  case urdf::Geometry::MESH: {
319  const urdf::Mesh& mesh = static_cast<const urdf::Mesh&>(geom);
320  makeMesh(m, mesh.filename, mesh.scale.x, mesh.scale.y, mesh.scale.z);
321  break;
322  }
323  default:
324  ROS_WARN("Unsupported geometry type: %d", geom.type);
325  break;
326  }
327 
328  return m;
329 }
330 
331 } // namespace rviz_marker_tools
rviz_marker_tools
Definition: marker_creation.h:12
rviz_marker_tools::getColor
std_msgs::ColorRGBA getColor(Color color, double alpha=1.0)
Definition: marker_creation.cpp:135
rviz_marker_tools::WHITE
@ WHITE
Definition: marker_creation.h:29
rviz_marker_tools::RED
@ RED
Definition: marker_creation.h:27
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
rviz_marker_tools::prepareMarker
void prepareMarker(vm::Marker &m, int marker_type)
Definition: marker_creation.cpp:155
rviz_marker_tools::LIME_GREEN
@ LIME_GREEN
Definition: marker_creation.h:23
rviz_marker_tools::brighten
std_msgs::ColorRGBA & brighten(std_msgs::ColorRGBA &color, double fraction)
Definition: marker_creation.cpp:123
rviz_marker_tools::GREY
@ GREY
Definition: marker_creation.h:20
rviz_marker_tools::makeSphere
visualization_msgs::Marker & makeSphere(visualization_msgs::Marker &m, double radius=1.0)
rviz_marker_tools::makeFromGeometry
visualization_msgs::Marker & makeFromGeometry(visualization_msgs::Marker &m, const urdf::Geometry &geom)
create marker from urdf::Geom
rviz_marker_tools::makeYZPlane
visualization_msgs::Marker & makeYZPlane(visualization_msgs::Marker &m)
rviz_marker_tools::interpolate
std_msgs::ColorRGBA & interpolate(std_msgs::ColorRGBA &color, const std_msgs::ColorRGBA &other, double fraction)
Definition: marker_creation.cpp:111
rviz_marker_tools::makeCylinder
visualization_msgs::Marker & makeCylinder(visualization_msgs::Marker &m, double diameter, double height)
create a cylinder along z-axis
console.h
rviz_marker_tools::composePoses
geometry_msgs::Pose composePoses(const geometry_msgs::Pose &first, const Eigen::Isometry3d &second)
Definition: marker_creation.cpp:141
rviz_marker_tools::PURPLE
@ PURPLE
Definition: marker_creation.h:26
marker_creation.h
rviz_marker_tools::makeMesh
visualization_msgs::Marker & makeMesh(visualization_msgs::Marker &m, const std::string &filename, double sx=1.0, double sy=1.0, double sz=1.0)
create a mesh marker
rviz_marker_tools::BLACK
@ BLACK
Definition: marker_creation.h:16
rviz_marker_tools::GREEN
@ GREEN
Definition: marker_creation.h:22
rviz_marker_tools::makeCone
visualization_msgs::Marker & makeCone(visualization_msgs::Marker &m, double angle)
create a cone of given angle along the x-axis
ROS_WARN
#define ROS_WARN(...)
rviz_marker_tools::makeArrow
visualization_msgs::Marker & makeArrow(visualization_msgs::Marker &m, const Eigen::Vector3d &start_point, const Eigen::Vector3d &end_point, double diameter, double head_length=0.0)
create an arrow with a start and end point
rviz_marker_tools::makeBox
visualization_msgs::Marker & makeBox(visualization_msgs::Marker &m, double x, double y, double z)
create a box with given dimensions along x, y, z axes
start
ROSCPP_DECL void start()
rviz_marker_tools::ORANGE
@ ORANGE
Definition: marker_creation.h:25
rviz_marker_tools::CYAN
@ CYAN
Definition: marker_creation.h:19
rviz_marker_tools::darken
std_msgs::ColorRGBA & darken(std_msgs::ColorRGBA &color, double fraction)
Definition: marker_creation.cpp:130
rviz_marker_tools::DARK_GREY
@ DARK_GREY
Definition: marker_creation.h:21
rviz_marker_tools::makeXZPlane
visualization_msgs::Marker & makeXZPlane(visualization_msgs::Marker &m)
angle
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
rviz_marker_tools::BLUE
@ BLUE
Definition: marker_creation.h:18
tf2::toMsg
B toMsg(const A &a)
rviz_marker_tools::Color
Color
Definition: marker_creation.h:14
rviz_marker_tools::BROWN
@ BROWN
Definition: marker_creation.h:17
rviz_marker_tools::MAGENTA
@ MAGENTA
Definition: marker_creation.h:24
rviz_marker_tools::setColor
std_msgs::ColorRGBA & setColor(std_msgs::ColorRGBA &color, Color color_id, double alpha=1.0)
Definition: marker_creation.cpp:10
rviz_marker_tools::YELLOW
@ YELLOW
Definition: marker_creation.h:30
rviz_marker_tools::PINK
@ PINK
Definition: marker_creation.h:28
rviz_marker_tools::makeText
visualization_msgs::Marker & makeText(visualization_msgs::Marker &m, const std::string &text)
create text marker
rviz_marker_tools::makeXYPlane
visualization_msgs::Marker & makeXYPlane(visualization_msgs::Marker &m)
create planes with corners (-1,-1) - (+1,+1)


rviz_marker_tools
Author(s): Robert Haschke
autogenerated on Thu Feb 27 2025 03:39:19