sick_scan_marker.cpp
Go to the documentation of this file.
1 /*
2  * @brief Implementation of object markers for sick_scan
3  *
4  * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim
5  * Copyright (C) 2021, SICK AG, Waldkirch
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions 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 copyright
14  * notice, this list of conditions and the following disclaimer in the
15  * documentation and/or other materials provided with the distribution.
16  * * Neither the name of Osnabrück University nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
24  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  * Created on: 13.01.2021
33  *
34  * Authors:
35  * Michael Lehning <michael.lehning@lehning.de>
36  *
37  */
41 
43 
44 static std_msgs::ColorRGBA color(float r, float g, float b, float a = 0.5f)
45 {
46  std_msgs::ColorRGBA color;
47  color.r = r;
48  color.g = g;
49  color.b = b;
50  color.a = a;
51  return color;
52 }
53 
54 static std_msgs::ColorRGBA red(void)
55 {
56  return color(1.0f, 0.0f, 0.0f);
57 }
58 
59 static std_msgs::ColorRGBA green(void) // free fields
60 {
61  return color(0.0f, 1.0f, 0.0f);
62 }
63 
64 static std_msgs::ColorRGBA blue(void)
65 {
66  return color(0.0f, 0.0f, 1.0f);
67 }
68 
69 static std_msgs::ColorRGBA yellow(void) // infringed fields
70 {
71  return color(1.0f, 1.0f, 0.0f);
72 }
73 
74 static std_msgs::ColorRGBA gray(void) // invalid fields (default)
75 {
76  return color(0.5f, 0.5f, 0.5f);
77 }
78 
79 sick_scan::SickScanMarker::SickScanMarker(ros::NodeHandle* nh, const std::string & marker_topic, const std::string & marker_frame_id)
80 : m_scan_mon_fieldset(0), m_marker_output_legend_offset_x(-0.5)
81 {
82  if(nh)
83  {
84  m_frame_id = marker_frame_id.empty() ? "/cloud" : marker_frame_id;
85  m_marker_publisher = nh->advertise<visualization_msgs::MarkerArray>(marker_topic.empty() ? "sick_scan/marker" : marker_topic, 1);
86  }
87 }
88 
90 {
91 }
92 
93 void sick_scan::SickScanMarker::updateMarker(const std::vector<SickScanMonField>& fields, int fieldset, int _eval_field_logic)
94 {
95  sick_scan::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan::EVAL_FIELD_SUPPORT)_eval_field_logic;
96  m_scan_mon_fields = fields;
97  if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC)
98  {
99  m_scan_mon_fieldset = fieldset;
100  std::vector<FieldInfo> default_fields = {FieldInfo(0,0,"-","3",gray()), FieldInfo(1,0,"-","2",gray()), FieldInfo(2,0,"-","1",gray())};
103  }
104  // if(eval_field_logic != USE_EVAL_FIELD_LMS5XX_LOGIC)
105  // m_scan_fieldset_legend = createMonFieldsetLegend(0);
106  // m_scan_outputstate_legend = createOutputStateLegend({"0", "0", "0"}, {"-", "-", "-"}, {gray(), gray(), gray()}); // only if outputstates active, i.e. after updateMarker(LIDoutputstateMsg)
107  publishMarker();
108 }
109 
110 void sick_scan::SickScanMarker::updateMarker(sick_scan::LIDoutputstateMsg& msg, int _eval_field_logic)
111 {
112  sick_scan::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan::EVAL_FIELD_SUPPORT)_eval_field_logic;
114  if(fieldMon && eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC)
115  {
117  ROS_DEBUG_STREAM("SickScanMarker: active_fieldset = " << fieldMon->getActiveFieldset());
118  }
119  int num_devices = std::min(msg.output_count.size(), msg.output_state.size());
120  std::vector<std::string> output_state(num_devices);
121  std::vector<std::string> output_count(num_devices);
122  std::vector<std_msgs::ColorRGBA> output_colors(num_devices);
123  for(int field_idx = 0; field_idx < num_devices; field_idx++)
124  {
125  int count = msg.output_count[field_idx];
126  int state = msg.output_state[field_idx];
127  output_state[field_idx] = std::to_string(state);
128  output_count[field_idx] = std::to_string(count);
129  if(state == 1) // 1 = active = yellow
130  {
131  output_state[field_idx] = "[ON]";
132  output_colors[field_idx] = yellow();
133  }
134  else // 0 = not active = gray or 2 = not used = gray
135  {
136  output_state[field_idx] = "[OFF]";
137  output_colors[field_idx] = gray();
138  }
139  }
140  std::stringstream dbg_info;
141  dbg_info << "SickScanMarker::updateMarker(): LIDoutputstateMsg (state,count) = { ";
142  for(int field_idx = 0; field_idx < num_devices; field_idx++)
143  dbg_info << ((field_idx > 0) ? ", (" : "(") << output_state[field_idx] << "," << output_count[field_idx] << ")";
144  dbg_info << " }";
145  ROS_DEBUG_STREAM(dbg_info.str());
146  if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC)
148  m_scan_outputstate_legend = createOutputStateLegend(output_state, output_count, output_colors);
149  publishMarker();
150 }
151 
152 void sick_scan::SickScanMarker::updateMarker(sick_scan::LFErecMsg& msg, int _eval_field_logic)
153 {
154  sick_scan::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan::EVAL_FIELD_SUPPORT)_eval_field_logic;
156  if(fieldMon && eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC)
157  {
159  ROS_DEBUG_STREAM("SickScanMarker: active_fieldset = " << fieldMon->getActiveFieldset());
160  }
161  std::vector<FieldInfo> field_info(msg.fields.size());
162  for(int field_idx = 0; field_idx < msg.fields.size(); field_idx++)
163  {
164  // LFErec: field_index runs from 1 to 3, field_info: field_index_scan_mon runs from 0 to 47 (field_index of m_scan_mon_fields)
165  field_info[field_idx].field_index_scan_mon = msg.fields[field_idx].field_index - 1 + msg.fields.size() * m_scan_mon_fieldset;
166  field_info[field_idx].field_result = msg.fields[field_idx].field_result_mrs;
167  if(field_info[field_idx].field_result == 1) // 1 = free/clear = green
168  {
169  field_info[field_idx].field_status = "Clear";
170  field_info[field_idx].field_color = green();
171  }
172  else if(field_info[field_idx].field_result == 2) // 2 = infringed = yellow
173  {
174  field_info[field_idx].field_status = "Infringed";
175  field_info[field_idx].field_color = yellow();
176  }
177  else // 0 = invalid = gray
178  {
179  field_info[field_idx].field_status = "Incorrect";
180  field_info[field_idx].field_color = gray();
181  }
182  if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC)
183  field_info[field_idx].field_name = std::to_string(field_info.size() - field_idx); // field_info[field_info_idx].field_index;
184  else
185  field_info[field_idx].field_name = std::to_string(msg.fields[field_idx].field_index);
186  }
187  std::stringstream dbg_info;
188  dbg_info << "SickScanMarker::updateMarker(): LFErec states={";
189  for(int field_idx = 0; field_idx < msg.fields.size(); field_idx++)
190  dbg_info << ((field_idx > 0) ? "," : "") << (int)msg.fields[field_idx].field_index << ":" << (int)msg.fields[field_idx].field_result_mrs;
191  dbg_info << "}, mon_field_point_cnt={";
192  for(int field_idx = 0; field_idx < m_scan_mon_fields.size(); field_idx++)
193  dbg_info << ((field_idx > 0) ? "," : "") << m_scan_mon_fields[field_idx].getPointCount();
194  dbg_info << "}, mon_field_set = " << m_scan_mon_fieldset;
195  ROS_DEBUG_STREAM(dbg_info.str());
198  if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC)
199  m_scan_fieldset_legend = createMonFieldsetLegend(m_scan_mon_fieldset);
200  publishMarker();
201 }
202 
204 {
205  visualization_msgs::MarkerArray marker_array;
206  marker_array.markers.reserve(m_scan_mon_field_marker.size() + m_scan_mon_field_legend.size() + m_scan_outputstate_legend.size());
207  for(int n = 0; n < m_scan_mon_field_marker.size(); n++)
208  marker_array.markers.push_back(m_scan_mon_field_marker[n]);
209  for(int n = 0; n < m_scan_mon_field_legend.size(); n++)
210  marker_array.markers.push_back(m_scan_mon_field_legend[n]);
211  for(int n = 0; n < m_scan_outputstate_legend.size(); n++)
212  marker_array.markers.push_back(m_scan_outputstate_legend[n]);
213  for(int n = 0; n < m_scan_fieldset_legend.size(); n++)
214  marker_array.markers.push_back(m_scan_fieldset_legend[n]);
215  m_marker_publisher.publish(marker_array);
216 }
217 
218 static void appendTrianglePoints(int point_count, const std::vector<float>& points_x, const std::vector<float>& points_y,
219  visualization_msgs::Marker& marker_point, int& triangle_idx, int nr_triangles, std_msgs::ColorRGBA field_color)
220 {
221  for(int point_idx = 2; point_idx < point_count && triangle_idx < nr_triangles; point_idx++, triangle_idx++)
222  {
223 
224  marker_point.points[3 * triangle_idx + 0].x = points_x[0];
225  marker_point.points[3 * triangle_idx + 0].y = points_y[0];
226  marker_point.points[3 * triangle_idx + 0].z = 0;
227 
228  marker_point.points[3 * triangle_idx + 1].x = points_x[point_idx - 1];
229  marker_point.points[3 * triangle_idx + 1].y = points_y[point_idx - 1];
230  marker_point.points[3 * triangle_idx + 1].z = 0;
231 
232  marker_point.points[3 * triangle_idx + 2].x = points_x[point_idx];
233  marker_point.points[3 * triangle_idx + 2].y = points_y[point_idx];
234  marker_point.points[3 * triangle_idx + 2].z = 0;
235 
236  marker_point.colors[3 * triangle_idx + 0] = field_color;
237  marker_point.colors[3 * triangle_idx + 1] = field_color;
238  marker_point.colors[3 * triangle_idx + 2] = field_color;
239  }
240 }
241 
242 std::vector<visualization_msgs::Marker> sick_scan::SickScanMarker::createMonFieldMarker(const std::vector<FieldInfo>& field_info)
243 {
244  int nr_triangles = 0;
245  for(int field_info_idx = 0; field_info_idx < field_info.size(); field_info_idx++)
246  {
247  int field_idx = field_info[field_info_idx].field_index_scan_mon;
248  SickScanMonFieldType field_typ = m_scan_mon_fields[field_idx].fieldType();
249  if(field_typ == MON_FIELD_DYNAMIC) // dynamic fields have two rectangle (first rectangle for v = max, second rectangle for v = 0)
250  nr_triangles += 2 * std::max(0, m_scan_mon_fields[field_idx].getPointCount()/2 - 2); // 3 points: 1 triangle, 4 points: 3 triangles, and so on
251  else
252  nr_triangles += std::max(0, m_scan_mon_fields[field_idx].getPointCount() - 2); // 3 points: 1 triangle, 4 points: 3 triangles, and so on
253  }
254 
255  // Draw fields using marker triangles
256  visualization_msgs::Marker marker_point;
257  marker_point.header.stamp = ros::Time::now();
258  marker_point.header.frame_id = m_frame_id;
259  marker_point.ns = "sick_scan";
260  marker_point.id = 1;
261  marker_point.type = visualization_msgs::Marker::TRIANGLE_LIST;
262  marker_point.scale.x = 1;
263  marker_point.scale.y = 1;
264  marker_point.scale.z = 1;
265  marker_point.pose.position.x = 0.0;
266  marker_point.pose.position.y = 0.0;
267  marker_point.pose.position.z = 0.0;
268  marker_point.pose.orientation.x = 0.0;
269  marker_point.pose.orientation.y = 0.0;
270  marker_point.pose.orientation.z = 0.0;
271  marker_point.pose.orientation.w = 1.0;
272  marker_point.action = visualization_msgs::Marker::ADD; // note: ADD == MODIFY
273  marker_point.color = gray();
274  marker_point.lifetime = ros::Duration(0); // lifetime 0 indicates forever
275 
276  marker_point.points.resize(3 * nr_triangles);
277  marker_point.colors.resize(3 * nr_triangles);
278  std::vector<geometry_msgs::Point> triangle_centroids;
279  triangle_centroids.reserve(field_info.size());
280  for(int field_info_idx = 0, triangle_idx = 0; field_info_idx < field_info.size() && triangle_idx < nr_triangles; field_info_idx++)
281  {
282  int field_idx = field_info[field_info_idx].field_index_scan_mon;
283  std_msgs::ColorRGBA field_color = field_info[field_info_idx].field_color;
284  int point_count = m_scan_mon_fields[field_idx].getPointCount();
285  const std::vector<float>& points_x = m_scan_mon_fields[field_idx].getFieldPointsX();
286  const std::vector<float>& points_y = m_scan_mon_fields[field_idx].getFieldPointsY();
287  SickScanMonFieldType field_typ = m_scan_mon_fields[field_idx].fieldType();
288  if(field_typ == MON_FIELD_DYNAMIC) // dynamic fields have two rectangle (first rectangle for v = max, second rectangle for v = 0)
289  {
290  std::vector<float> field1_points_x(point_count/2), field1_points_y(point_count/2), field2_points_x(point_count/2), field2_points_y(point_count/2);
291  for(int n = 0; n < point_count/2; n++)
292  {
293  field1_points_x[n] = points_x[n];
294  field1_points_y[n] = points_y[n];
295  field2_points_x[n] = points_x[n + point_count/2];
296  field2_points_y[n] = points_y[n + point_count/2];
297  }
298  std_msgs::ColorRGBA field1_color = field_color, field2_color = field_color;
299  field1_color.r *= 0.5;
300  field1_color.g *= 0.5;
301  field1_color.b *= 0.5;
302  appendTrianglePoints(point_count/2, field1_points_x, field1_points_y, marker_point, triangle_idx, nr_triangles, field1_color);
303  appendTrianglePoints(point_count/2, field2_points_x, field2_points_y, marker_point, triangle_idx, nr_triangles, field2_color);
304  }
305  else
306  {
307  appendTrianglePoints(point_count, points_x, points_y, marker_point, triangle_idx, nr_triangles, field_color);
308  }
309  }
310 
311  std::vector<visualization_msgs::Marker> marker_array;
312  marker_array.reserve(1 + field_info.size());
313  marker_array.push_back(marker_point);
314 
315  // Draw field names
316  for(int field_info_idx = 0; field_info_idx < field_info.size(); field_info_idx++)
317  {
318  int field_idx = field_info[field_info_idx].field_index_scan_mon;
319  if(m_scan_mon_fields[field_idx].getPointCount() >= 3)
320  {
321  geometry_msgs::Point triangle_centroid;
322  triangle_centroid.x = 0;
323  triangle_centroid.y = 0;
324  triangle_centroid.z = 0;
325  const std::vector<float>& points_x = m_scan_mon_fields[field_idx].getFieldPointsX();
326  const std::vector<float>& points_y = m_scan_mon_fields[field_idx].getFieldPointsY();
327  for(int point_idx = 0; point_idx < m_scan_mon_fields[field_idx].getPointCount(); point_idx++)
328  {
329  triangle_centroid.x += points_x[point_idx];
330  triangle_centroid.y += points_y[point_idx];
331  }
332  triangle_centroid.x /= (float)(m_scan_mon_fields[field_idx].getPointCount());
333  triangle_centroid.y /= (float)(m_scan_mon_fields[field_idx].getPointCount());
334  visualization_msgs::Marker marker_field_name;
335  marker_field_name.header.stamp = ros::Time::now();
336  marker_field_name.header.frame_id = m_frame_id;
337  marker_field_name.ns = "sick_scan";
338  marker_field_name.id = 2 + field_info_idx;
339  marker_field_name.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
340  marker_field_name.scale.z = 0.1;
341  marker_field_name.pose.position.x = triangle_centroid.x;
342  marker_field_name.pose.position.y = triangle_centroid.y;
343  marker_field_name.pose.position.z = triangle_centroid.z;
344  marker_field_name.pose.orientation.x = 0.0;
345  marker_field_name.pose.orientation.y = 0.0;
346  marker_field_name.pose.orientation.z = 0.0;
347  marker_field_name.pose.orientation.w = 1.0;
348  marker_field_name.action = visualization_msgs::Marker::ADD; // note: ADD == MODIFY
349  marker_field_name.color = field_info[field_info_idx].field_color;
350  marker_field_name.color.a = 1;
351  marker_field_name.lifetime = ros::Duration(0); // lifetime 0 indicates forever
352  marker_field_name.text = field_info[field_info_idx].field_name;
353  marker_array.push_back(marker_field_name);
354  }
355  else
356  {
357  visualization_msgs::Marker marker_field_name;
358  marker_field_name.header.stamp = ros::Time::now();
359  marker_field_name.header.frame_id = m_frame_id;
360  marker_field_name.ns = "sick_scan";
361  marker_field_name.id = 2 + field_info_idx;
362  marker_field_name.action = visualization_msgs::Marker::DELETE;
363  marker_field_name.lifetime = ros::Duration(0); // lifetime 0 indicates forever
364  marker_array.push_back(marker_field_name);
365  }
366 
367  }
368 
369  return marker_array;
370 }
371 
372 std::vector<visualization_msgs::Marker> sick_scan::SickScanMarker::createMonFieldLegend(const std::vector<FieldInfo>& field_info)
373 {
374  std::vector<visualization_msgs::Marker> marker_array;
375  marker_array.reserve(2 * field_info.size());
376  for(int loop_cnt = 0; loop_cnt < 2; loop_cnt++)
377  {
378  for(int field_info_idx = 0, triangle_idx = 0; field_info_idx < field_info.size(); field_info_idx++)
379  {
380  int field_idx = field_info[field_info_idx].field_index_scan_mon;
381  visualization_msgs::Marker marker_point;
382  marker_point.header.stamp = ros::Time::now();
383  marker_point.header.frame_id = m_frame_id;
384  marker_point.ns = "sick_scan";
385  marker_point.id = 100 + loop_cnt * m_scan_mon_fields.size() + field_info_idx;
386  marker_point.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
387  marker_point.scale.z = 0.1;
388  marker_point.pose.position.x = -0.1 * field_info_idx - 0.1;
389  marker_point.pose.position.y = ((loop_cnt == 0) ? 0.3 : -0.2);
390  marker_point.pose.position.z = 0.0;
391  marker_point.pose.orientation.x = 0.0;
392  marker_point.pose.orientation.y = 0.0;
393  marker_point.pose.orientation.z = 0.0;
394  marker_point.pose.orientation.w = 1.0;
395  marker_point.action = visualization_msgs::Marker::ADD; // note: ADD == MODIFY
396  marker_point.color = field_info[field_info_idx].field_color;
397  marker_point.color.a = 1;
398  marker_point.lifetime = ros::Duration(0); // lifetime 0 indicates forever
399  std::stringstream marker_text;
400  // int detection_field_number = field_info.size() - field_info_idx; // field_info[field_info_idx].field_index;
401  if (loop_cnt == 0)
402  marker_text << "Detection field " << (field_info[field_info_idx].field_name) << " : ";
403  else
404  marker_text << field_info[field_info_idx].field_status;
405  marker_point.text = marker_text.str();
406  marker_array.push_back(marker_point);
407  if(m_marker_output_legend_offset_x > marker_point.pose.position.x - 0.1)
408  m_marker_output_legend_offset_x = marker_point.pose.position.x - 0.1;
409  }
410  }
411  return marker_array;
412 }
413 
414 std::vector<visualization_msgs::Marker> sick_scan::SickScanMarker::createMonFieldsetLegend(int fieldset)
415 {
416  std::vector<visualization_msgs::Marker> marker_array;
417  marker_array.reserve(2);
418  for(int loop_cnt = 0; loop_cnt < 2; loop_cnt++)
419  {
420  visualization_msgs::Marker marker_point;
421  marker_point.header.stamp = ros::Time::now();
422  marker_point.header.frame_id = m_frame_id;
423  marker_point.ns = "sick_scan";
424  marker_point.id = 500 + loop_cnt;
425  marker_point.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
426  marker_point.scale.z = 0.1;
427  marker_point.pose.position.x = -0.4;
428  marker_point.pose.position.y = ((loop_cnt == 0) ? 0.16 : -0.2);
429  marker_point.pose.position.z = 0.0;
430  marker_point.pose.orientation.x = 0.0;
431  marker_point.pose.orientation.y = 0.0;
432  marker_point.pose.orientation.z = 0.0;
433  marker_point.pose.orientation.w = 1.0;
434  marker_point.action = visualization_msgs::Marker::ADD; // note: ADD == MODIFY
435  marker_point.color = green();
436  marker_point.color.a = 1;
437  marker_point.lifetime = ros::Duration(0); // lifetime 0 indicates forever
438  std::stringstream marker_text;
439  if (loop_cnt == 0)
440  marker_text << "Fieldset :";
441  else
442  marker_text << std::to_string(fieldset + 1);
443  marker_point.text = marker_text.str();
444  marker_array.push_back(marker_point);
445  }
446  return marker_array;
447 }
448 
449 
450 std::vector<visualization_msgs::Marker> sick_scan::SickScanMarker::createOutputStateLegend(const std::vector<std::string>& output_state, const std::vector<std::string>& output_count, const std::vector<std_msgs::ColorRGBA>& output_colors)
451 {
452  std::vector<visualization_msgs::Marker> marker_array;
453  marker_array.reserve(2 * output_count.size());
454  for(int loop_cnt = 0; loop_cnt < 2; loop_cnt++)
455  {
456  for(int field_idx = 0; field_idx < output_count.size(); field_idx++)
457  {
458  visualization_msgs::Marker marker_point;
459  marker_point.header.stamp = ros::Time::now();
460  marker_point.header.frame_id = m_frame_id;
461  marker_point.ns = "sick_scan";
462  marker_point.id = 400 + loop_cnt * output_count.size() + field_idx;
463  marker_point.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
464  marker_point.scale.z = 0.1;
465  marker_point.pose.position.x = -0.1 * field_idx + m_marker_output_legend_offset_x;
466  marker_point.pose.position.y = ((loop_cnt == 0) ? 0.16 : -0.3);
467  marker_point.pose.position.z = 0.0;
468  marker_point.pose.orientation.x = 0.0;
469  marker_point.pose.orientation.y = 0.0;
470  marker_point.pose.orientation.z = 0.0;
471  marker_point.pose.orientation.w = 1.0;
472  marker_point.action = visualization_msgs::Marker::ADD; // note: ADD == MODIFY
473  marker_point.color = output_colors[field_idx];
474  marker_point.color.a = 1;
475  marker_point.lifetime = ros::Duration(0); // lifetime 0 indicates forever
476  std::stringstream marker_text;
477  int output_device = field_idx + 1;
478  if (loop_cnt == 0)
479  marker_text << "Output " << output_device << " : ";
480  else
481  marker_text << (field_idx < output_state.size() ? (output_state[field_idx]) : "") << " Count:" << output_count[field_idx];
482  marker_point.text = marker_text.str();
483  marker_array.push_back(marker_point);
484  }
485  }
486  return marker_array;
487 }
std::vector< visualization_msgs::Marker > createMonFieldsetLegend(int fieldset)
std::vector< visualization_msgs::Marker > m_scan_outputstate_legend
SickScanMarker(ros::NodeHandle *nh=0, const std::string &marker_topic="", const std::string &marker_frame_id="")
void updateMarker(const std::vector< SickScanMonField > &fields, int fieldset, int eval_field_logic)
static std_msgs::ColorRGBA red(void)
f
std::vector< visualization_msgs::Marker > m_scan_mon_field_marker
std::vector< sick_scan::SickScanMonField > m_scan_mon_fields
static void appendTrianglePoints(int point_count, const std::vector< float > &points_x, const std::vector< float > &points_y, visualization_msgs::Marker &marker_point, int &triangle_idx, int nr_triangles, std_msgs::ColorRGBA field_color)
static std_msgs::ColorRGBA gray(void)
void publish(const boost::shared_ptr< M > &message) const
static SickScanFieldMonSingleton * getInstance()
std::vector< visualization_msgs::Marker > createMonFieldLegend(const std::vector< FieldInfo > &field_info)
static std_msgs::ColorRGBA blue(void)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static std_msgs::ColorRGBA color(float r, float g, float b, float a=0.5f)
#define ROS_DEBUG_STREAM(args)
std::vector< visualization_msgs::Marker > createMonFieldMarker(const std::vector< FieldInfo > &field_info)
static std_msgs::ColorRGBA green(void)
std::vector< visualization_msgs::Marker > createOutputStateLegend(const std::vector< std::string > &output_state, const std::vector< std::string > &output_count, const std::vector< std_msgs::ColorRGBA > &output_colors)
static std_msgs::ColorRGBA yellow(void)
static Time now()
std::vector< visualization_msgs::Marker > m_scan_mon_field_legend
std::vector< visualization_msgs::Marker > m_scan_fieldset_legend
ros::Publisher m_marker_publisher


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed Sep 7 2022 02:25:06