pie_chart_display.cpp
Go to the documentation of this file.
1 // -*- mode: c++; -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
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
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #include "pie_chart_display.h"
37 
38 #include <OGRE/OgreMaterialManager.h>
39 #include <OGRE/OgreTextureManager.h>
40 #include <OGRE/OgreTexture.h>
41 #include <OGRE/OgreTechnique.h>
42 #include <OGRE/OgreHardwarePixelBuffer.h>
43 #include <rviz/uniform_string_stream.h>
44 #include <rviz/display_context.h>
45 #include <QPainter>
46 
47 namespace jsk_rviz_plugins
48 {
49 
50  PieChartDisplay::PieChartDisplay()
51  : rviz::Display(), update_required_(false), first_time_(true), data_(0.0)
52  {
53  update_topic_property_ = new rviz::RosTopicProperty(
54  "Topic", "",
55  ros::message_traits::datatype<std_msgs::Float32>(),
56  "std_msgs::Float32 topic to subscribe to.",
57  this, SLOT( updateTopic() ));
58  size_property_ = new rviz::IntProperty("size", 128,
59  "size of the plotter window",
60  this, SLOT(updateSize()));
61  left_property_ = new rviz::IntProperty("left", 128,
62  "left of the plotter window",
63  this, SLOT(updateLeft()));
64  top_property_ = new rviz::IntProperty("top", 128,
65  "top of the plotter window",
66  this, SLOT(updateTop()));
67  fg_color_property_ = new rviz::ColorProperty("foreground color",
68  QColor(25, 255, 240),
69  "color to draw line",
70  this, SLOT(updateFGColor()));
71  fg_alpha_property_
72  = new rviz::FloatProperty("foreground alpha", 0.7,
73  "alpha belnding value for foreground",
74  this, SLOT(updateFGAlpha()));
75  fg_alpha2_property_
76  = new rviz::FloatProperty("foreground alpha 2", 0.4,
77  "alpha belnding value for foreground for indicator",
78  this, SLOT(updateFGAlpha2()));
79  bg_color_property_ = new rviz::ColorProperty("background color",
80  QColor(0, 0, 0),
81  "background color",
82  this, SLOT(updateBGColor()));
83  bg_alpha_property_
84  = new rviz::FloatProperty("backround alpha", 0.0,
85  "alpha belnding value for background",
86  this, SLOT(updateBGAlpha()));
87  text_size_property_
88  = new rviz::IntProperty("text size", 14,
89  "text size",
90  this, SLOT(updateTextSize()));
91  show_caption_property_
92  = new rviz::BoolProperty("show caption", true,
93  "show caption",
94  this, SLOT(updateShowCaption()));
95  max_value_property_
96  = new rviz::FloatProperty("max value", 1.0,
97  "max value of pie chart",
98  this, SLOT(updateMaxValue()));
99  min_value_property_
100  = new rviz::FloatProperty("min value", 0.0,
101  "min value of pie chart",
102  this, SLOT(updateMinValue()));
103  auto_color_change_property_
104  = new rviz::BoolProperty("auto color change",
105  false,
106  "change the color automatically",
107  this, SLOT(updateAutoColorChange()));
108  max_color_property_
109  = new rviz::ColorProperty("max color",
110  QColor(255, 0, 0),
111  "only used if auto color change is set to True.",
112  this, SLOT(updateMaxColor()));
113 
114  med_color_property_
115  = new rviz::ColorProperty("med color",
116  QColor(255, 0, 0),
117  "only used if auto color change is set to True.",
118  this, SLOT(updateMedColor()));
119 
120  max_color_threshold_property_
121  = new rviz::FloatProperty("max color change threthold", 0,
122  "change the max color at threshold",
123  this, SLOT(updateMaxColorThreshold()));
124 
125  med_color_threshold_property_
126  = new rviz::FloatProperty("med color change threthold", 0,
127  "change the med color at threshold ",
128  this, SLOT(updateMedColorThreshold()));
129 
130  clockwise_rotate_property_
131  = new rviz::BoolProperty("clockwise rotate direction",
132  false,
133  "change the rotate direction",
134  this, SLOT(updateClockwiseRotate()));
135  }
136 
137  PieChartDisplay::~PieChartDisplay()
138  {
139  if (overlay_->isVisible()) {
140  overlay_->hide();
141  }
142  delete update_topic_property_;
143  delete fg_color_property_;
144  delete bg_color_property_;
145  delete fg_alpha_property_;
146  delete fg_alpha2_property_;
147  delete bg_alpha_property_;
148  delete top_property_;
149  delete left_property_;
150  delete size_property_;
151  delete min_value_property_;
152  delete max_value_property_;
153  delete max_color_property_;
154  delete med_color_property_;
155  delete text_size_property_;
156  delete show_caption_property_;
157  }
158 
159  void PieChartDisplay::onInitialize()
160  {
161  static int count = 0;
162  rviz::UniformStringStream ss;
163  ss << "PieChartDisplayObject" << count++;
164  overlay_.reset(new OverlayObject(ss.str()));
165  onEnable();
166  updateSize();
167  updateLeft();
168  updateTop();
169  updateFGColor();
170  updateBGColor();
171  updateFGAlpha();
172  updateFGAlpha2();
173  updateBGAlpha();
174  updateMinValue();
175  updateMaxValue();
176  updateTextSize();
177  updateShowCaption();
178  updateAutoColorChange();
179  updateMaxColor();
180  updateMedColor();
181  updateMaxColorThreshold();
182  updateMedColorThreshold();
183  updateClockwiseRotate();
184  overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_);
185  overlay_->hide();
186  }
187 
188  void PieChartDisplay::update(float wall_dt, float ros_dt)
189  {
190  if (update_required_) {
191  update_required_ = false;
192  overlay_->updateTextureSize(texture_size_, texture_size_ + caption_offset_);
193  overlay_->setPosition(left_, top_);
194  overlay_->setDimensions(overlay_->getTextureWidth(),
195  overlay_->getTextureHeight());
196  drawPlot(data_);
197  }
198  }
199 
200  void PieChartDisplay::processMessage(const std_msgs::Float32::ConstPtr& msg)
201  {
202  boost::mutex::scoped_lock lock(mutex_);
203 
204  if (!overlay_->isVisible()) {
205  return;
206  }
207  if (data_ != msg->data || first_time_) {
208  first_time_ = false;
209  data_ = msg->data;
210  update_required_ = true;
211  }
212  }
213 
214  void PieChartDisplay::drawPlot(double val)
215  {
216  QColor fg_color(fg_color_);
217 
218  if (auto_color_change_) {
219  double r
220  = std::min(1.0, fabs((val - min_value_) / (max_value_ - min_value_)));
221  if (r > 0.6) {
222  double r2 = (r - 0.6) / 0.4;
223  fg_color.setRed((max_color_.red() - fg_color_.red()) * r2
224  + fg_color_.red());
225  fg_color.setGreen((max_color_.green() - fg_color_.green()) * r2
226  + fg_color_.green());
227  fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r2
228  + fg_color_.blue());
229  }
230  if (max_color_threshold_ != 0) {
231  if (r > max_color_threshold_) {
232  fg_color.setRed(max_color_.red());
233  fg_color.setGreen(max_color_.green());
234  fg_color.setBlue(max_color_.blue());
235  }
236  }
237  if (med_color_threshold_ != 0) {
238  if (max_color_threshold_ > r and r > med_color_threshold_ ) {
239  fg_color.setRed(med_color_.red());
240  fg_color.setGreen(med_color_.green());
241  fg_color.setBlue(med_color_.blue());
242  }
243  }
244  }
245 
246 
247  QColor fg_color2(fg_color);
248  QColor bg_color(bg_color_);
249  fg_color.setAlpha(fg_alpha_);
250  fg_color2.setAlpha(fg_alpha2_);
251  bg_color.setAlpha(bg_alpha_);
252  int width = overlay_->getTextureWidth();
253  int height = overlay_->getTextureHeight();
254  {
255  ScopedPixelBuffer buffer = overlay_->getBuffer();
256  QImage Hud = buffer.getQImage(*overlay_, bg_color);
257  QPainter painter( &Hud );
258  painter.setRenderHint(QPainter::Antialiasing, true);
259 
260  const int outer_line_width = 5;
261  const int value_line_width = 10;
262  const int value_indicator_line_width = 2;
263  const int value_padding = 5;
264 
265  const int value_aabb_offset
266  = outer_line_width + value_padding + value_line_width / 2;
267 
268  painter.setPen(QPen(fg_color, outer_line_width, Qt::SolidLine));
269 
270  painter.drawEllipse(outer_line_width / 2, outer_line_width / 2,
271  width - outer_line_width ,
272  height - outer_line_width - caption_offset_);
273 
274  painter.setPen(QPen(fg_color2, value_indicator_line_width, Qt::SolidLine));
275  painter.drawEllipse(value_aabb_offset, value_aabb_offset,
276  width - value_aabb_offset * 2,
277  height - value_aabb_offset * 2 - caption_offset_);
278 
279  const double ratio = (val - min_value_) / (max_value_ - min_value_);
280  const double rotate_direction = clockwise_rotate_ ? -1.0 : 1.0;
281  const double ratio_angle = ratio * 360.0 * rotate_direction;
282  const double start_angle_offset = -90;
283  painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine));
284  painter.drawArc(QRectF(value_aabb_offset, value_aabb_offset,
285  width - value_aabb_offset * 2,
286  height - value_aabb_offset * 2 - caption_offset_),
287  start_angle_offset * 16 ,
288  ratio_angle * 16);
289  QFont font = painter.font();
290  font.setPointSize(text_size_);
291  font.setBold(true);
292  painter.setFont(font);
293  painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine));
294  std::ostringstream s;
295  s << std::fixed << std::setprecision(2) << val;
296  painter.drawText(0, 0, width, height - caption_offset_,
297  Qt::AlignCenter | Qt::AlignVCenter,
298  s.str().c_str());
299 
300  // caption
301  if (show_caption_) {
302  painter.drawText(0, height - caption_offset_, width, caption_offset_,
303  Qt::AlignCenter | Qt::AlignVCenter,
304  getName());
305  }
306 
307  // done
308  painter.end();
309  // Unlock the pixel buffer
310  }
311  }
312 
313 
314  void PieChartDisplay::subscribe()
315  {
316  std::string topic_name = update_topic_property_->getTopicStd();
317  if (topic_name.length() > 0 && topic_name != "/") {
318  ros::NodeHandle n;
319  sub_ = n.subscribe(topic_name, 1, &PieChartDisplay::processMessage, this);
320  }
321  }
322 
323 
324  void PieChartDisplay::unsubscribe()
325  {
326  sub_.shutdown();
327  }
328 
329  void PieChartDisplay::onEnable()
330  {
331  subscribe();
332  overlay_->show();
333  first_time_ = true;
334  }
335 
336  void PieChartDisplay::onDisable()
337  {
338  unsubscribe();
339  overlay_->hide();
340  }
341 
342  void PieChartDisplay::updateSize()
343  {
344  boost::mutex::scoped_lock lock(mutex_);
345  texture_size_ = size_property_->getInt();
346  update_required_ = true;
347  }
348 
349  void PieChartDisplay::updateTop()
350  {
351  top_ = top_property_->getInt();
352  update_required_ = true;
353  }
354 
355  void PieChartDisplay::updateLeft()
356  {
357  left_ = left_property_->getInt();
358  update_required_ = true;
359  }
360 
361  void PieChartDisplay::updateBGColor()
362  {
363  bg_color_ = bg_color_property_->getColor();
364  update_required_ = true;
365 
366  }
367 
368  void PieChartDisplay::updateFGColor()
369  {
370  fg_color_ = fg_color_property_->getColor();
371  update_required_ = true;
372 
373  }
374 
375  void PieChartDisplay::updateFGAlpha()
376  {
377  fg_alpha_ = fg_alpha_property_->getFloat() * 255.0;
378  update_required_ = true;
379 
380  }
381 
382  void PieChartDisplay::updateFGAlpha2()
383  {
384  fg_alpha2_ = fg_alpha2_property_->getFloat() * 255.0;
385  update_required_ = true;
386 
387  }
388 
389 
390  void PieChartDisplay::updateBGAlpha()
391  {
392  bg_alpha_ = bg_alpha_property_->getFloat() * 255.0;
393  update_required_ = true;
394 
395  }
396 
397  void PieChartDisplay::updateMinValue()
398  {
399  min_value_ = min_value_property_->getFloat();
400  update_required_ = true;
401 
402  }
403 
404  void PieChartDisplay::updateMaxValue()
405  {
406  max_value_ = max_value_property_->getFloat();
407  update_required_ = true;
408 
409  }
410 
411  void PieChartDisplay::updateTextSize()
412  {
413  boost::mutex::scoped_lock lock(mutex_);
414  text_size_ = text_size_property_->getInt();
415  QFont font;
416  font.setPointSize(text_size_);
417  caption_offset_ = QFontMetrics(font).height();
418  update_required_ = true;
419 
420  }
421 
422  void PieChartDisplay::updateShowCaption()
423  {
424  show_caption_ = show_caption_property_->getBool();
425  update_required_ = true;
426 
427  }
428 
429 
430  void PieChartDisplay::updateTopic()
431  {
432  unsubscribe();
433  subscribe();
434  }
435 
436  void PieChartDisplay::updateAutoColorChange()
437  {
438  auto_color_change_ = auto_color_change_property_->getBool();
439  if (auto_color_change_) {
440  max_color_property_->show();
441  med_color_property_->show();
442  max_color_threshold_property_->show();
443  med_color_threshold_property_->show();
444  }
445  else {
446  max_color_property_->hide();
447  med_color_property_->hide();
448  max_color_threshold_property_->hide();
449  med_color_threshold_property_->hide();
450  }
451  update_required_ = true;
452 
453  }
454 
455  void PieChartDisplay::updateMaxColor()
456  {
457  max_color_ = max_color_property_->getColor();
458  update_required_ = true;
459 
460  }
461 
462  void PieChartDisplay::updateMedColor()
463  {
464  med_color_ = med_color_property_->getColor();
465  update_required_ = true;
466 
467  }
468 
469  void PieChartDisplay::updateMaxColorThreshold()
470  {
471  max_color_threshold_ = max_color_threshold_property_->getFloat();
472  update_required_ = true;
473  }
474 
475  void PieChartDisplay::updateMedColorThreshold()
476  {
477  med_color_threshold_ = med_color_threshold_property_->getFloat();
478  update_required_ = true;
479  }
480 
481  void PieChartDisplay::updateClockwiseRotate()
482  {
483  clockwise_rotate_ = clockwise_rotate_property_->getBool();
484  update_required_ = true;
485 
486  }
487 
488  bool PieChartDisplay::isInRegion(int x, int y)
489  {
490  return (top_ < y && top_ + texture_size_ > y &&
491  left_ < x && left_ + texture_size_ > x);
492  }
493 
494  void PieChartDisplay::movePosition(int x, int y)
495  {
496  top_ = y;
497  left_ = x;
498  }
499 
500  void PieChartDisplay::setPosition(int x, int y)
501  {
502  top_property_->setValue(y);
503  left_property_->setValue(x);
504  }
505 }
506 
507 #include <pluginlib/class_list_macros.h>
508 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::PieChartDisplay, rviz::Display )
unsubscribe
void unsubscribe()
rviz::RosTopicProperty
msg
msg
jsk_rviz_plugins::ScopedPixelBuffer
Definition: overlay_utils.h:101
rviz_mouse_point_to_tablet.lock
lock
Definition: rviz_mouse_point_to_tablet.py:11
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
rviz::BoolProperty
ros::Subscriber::shutdown
void shutdown()
overlay_sample.bg_color
bg_color
Definition: overlay_sample.py:45
rviz::UniformStringStream
rviz::ColorProperty
jsk_rviz_plugins::PieChartDisplay
Definition: pie_chart_display.h:85
rviz::Display
rviz::FloatProperty
class_list_macros.h
overlay_sample.font
font
Definition: overlay_sample.py:30
pie_chart_display.h
rviz
bounding_box_sample.x
x
Definition: bounding_box_sample.py:30
bounding_box_sample.r
r
Definition: bounding_box_sample.py:10
subscribe
void subscribe()
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
bounding_box_sample.y
y
Definition: bounding_box_sample.py:31
width
width
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
point_test.count
int count
Definition: point_test.py:15
jsk_rviz_plugins::ScopedPixelBuffer::getQImage
virtual QImage getQImage(unsigned int width, unsigned int height)
Definition: overlay_utils.cpp:89
motor_states_temperature_decomposer.s
s
Definition: motor_states_temperature_decomposer.py:79
height
height
jsk_rviz_plugins
Definition: __init__.py:1
jsk_rviz_plugins::PieChartDisplay::PieChartDisplay
PieChartDisplay()
Definition: pie_chart_display.cpp:82
ros::NodeHandle
uniform_string_stream.h
overlay_sample.fg_color
fg_color
Definition: overlay_sample.py:44
rviz::IntProperty
display_context.h

jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Mon Apr 7 2025 02:49:53