overlay_diagnostic_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/o2r 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 
37 
38 #include <OGRE/OgreMaterialManager.h>
39 #include <OGRE/OgreTextureManager.h>
40 #include <OGRE/OgreTexture.h>
41 #include <OGRE/OgreHardwarePixelBuffer.h>
42 #include <OGRE/OgreTechnique.h>
43 
45 #include <rviz/display_context.h>
46 #include <rviz/view_manager.h>
47 #include <rviz/render_panel.h>
48 
49 namespace jsk_rviz_plugins
50 {
54  : previous_state_(STALL_STATE), Display()
55  {
57  "Topic", "/diagnostics_agg",
58  ros::message_traits::datatype<diagnostic_msgs::DiagnosticArray>(),
59  "diagnostic_msgs::DiagnosticArray topic to subscribe to.",
60  this, SLOT( updateRosTopic() ));
62  "diagnostics namespace", "/",
63  "diagnostics namespace to visualize diagnostics",
64  this, SLOT(updateDiagnosticsNamespace()));
66  "type", "SAC", "Type of visualization", this, SLOT(updateType()));
67  type_property_->addOptionStd("SAC", 0);
68  type_property_->addOptionStd("EVA", 1);
70  "top", 128,
71  "top positoin",
72  this, SLOT(updateTop()));
74  "left", 128,
75  "left positoin",
76  this, SLOT(updateLeft()));
78  "size", 128,
79  "size of the widget",
80  this, SLOT(updateSize()));
83  "alpha", 0.8,
84  "alpha value",
85  this, SLOT(updateAlpha()));
86  alpha_property_->setMin(0.0);
87  alpha_property_->setMax(1.0);
89  "stall duration", 5.0,
90  "seconds to be regarded as stalled",
91  this, SLOT(updateStallDuration())
92  );
94  }
95 
97  {
98  if (overlay_) {
99  overlay_->hide();
100  }
101  // panel_material_->unload();
102  // Ogre::MaterialManager::getSingleton().remove(panel_material_->getName());
103  delete ros_topic_property_;
105  delete top_property_;
106  delete left_property_;
107  delete alpha_property_;
108  delete size_property_;
109  delete type_property_;
110  }
111 
113  const diagnostic_msgs::DiagnosticArray::ConstPtr& msg)
114  {
115  //std::make_shared<diagnostic_msgs::DiagnosticStatus>
116  // update namespaces_ if needed
117  std::set<std::string> new_namespaces;
118  for (size_t i = 0; i < msg->status.size(); i++) {
119  new_namespaces.insert(msg->status[i].name);
120  }
121 
122  std::set<std::string> difference_namespaces;
123  std::set_difference(namespaces_.begin(), namespaces_.end(),
124  new_namespaces.begin(), new_namespaces.end(),
125  std::inserter(difference_namespaces,
126  difference_namespaces.end()));
127  if (difference_namespaces.size() != 0) {
128  namespaces_ = new_namespaces;
130  }
131  else {
132  difference_namespaces.clear();
133  std::set_difference(new_namespaces.begin(), new_namespaces.end(),
134  namespaces_.begin(), namespaces_.end(),
135  std::inserter(difference_namespaces,
136  difference_namespaces.end()));
137  if (difference_namespaces.size() != 0) {
138  namespaces_ = new_namespaces;
140  }
141  }
142 
143  if (diagnostics_namespace_.length() == 0) {
144  return;
145  }
146 
147  for (size_t i = 0; i < msg->status.size(); i++) {
148  diagnostic_msgs::DiagnosticStatus status = msg->status[i];
149  if (status.name == diagnostics_namespace_) {
150 #if ROS_VERSION_MINIMUM(1,12,0)
152  = std::make_shared<diagnostic_msgs::DiagnosticStatus>(status);
153 #else
155  = boost::make_shared<diagnostic_msgs::DiagnosticStatus>(status);
156 #endif
158  break;
159  }
160  }
161  }
162 
163  void OverlayDiagnosticDisplay::update(float wall_dt, float ros_dt)
164  {
165  if (!isEnabled()) {
166  return;
167  }
168  if (!overlay_) {
169  static int count = 0;
171  ss << "OverlayDiagnosticDisplayObject" << count++;
172  overlay_.reset(new OverlayObject(ss.str()));
173  overlay_->show();
175  }
176  t_ += wall_dt;
177 
178  // check if the widget should animate
179  if (!is_animating_) {
180  if (previous_state_ != getLatestState()) {
181  is_animating_ = true;
183  }
184  }
185  else {
186  if (!isAnimating()) { // animation time is over
187  is_animating_ = false;
189  }
190  }
191 
192  overlay_->updateTextureSize(size_, size_);
193  redraw();
194  overlay_->setDimensions(overlay_->getTextureWidth(),
195  overlay_->getTextureHeight());
196  overlay_->setPosition(left_, top_);
197  t_ = fmod(t_, overlay_diagnostic_animation_duration);
198  }
199 
201  {
202  return ((ros::WallTime::now() - animation_start_time_).toSec() < overlay_diagnostic_animation_transition_duration);
203  }
204 
206  {
207  return ((ros::WallTime::now() - animation_start_time_).toSec() / overlay_diagnostic_animation_transition_duration);
208  }
209 
211  {
212  t_ = 0.0;
213  if (overlay_) {
214  overlay_->show();
215  }
216  subscribe();
217  }
218 
220  {
221  ROS_INFO("onDisable");
222  if (overlay_) {
223  overlay_->hide();
224  }
225  unsubscribe();
226  }
227 
229  {
230 
231  ROS_DEBUG("onInitialize");
232  updateType();
234  updateSize();
235  updateAlpha();
236  updateLeft();
237  updateTop();
239  updateRosTopic();
240  }
241 
243  {
244  sub_.shutdown();
245  }
246 
248  {
249  ros::NodeHandle n;
251  1,
253  this);
254  }
255 
257  {
258  if (latest_status_) {
259  ros::WallDuration message_duration
261  if (message_duration.toSec() < stall_duration_) {
262  return false;
263  }
264  else {
265  return true;
266  }
267  }
268  else {
269  return true;
270  }
271  }
272 
274  {
275  if (latest_status_) {
276  if (!isStalled()) {
277  if (latest_status_->level == diagnostic_msgs::DiagnosticStatus::OK) {
278  return "OK";
279  }
280  else if (latest_status_->level == diagnostic_msgs::DiagnosticStatus::WARN) {
281  return "WARN";
282  }
283  else if (latest_status_->level == diagnostic_msgs::DiagnosticStatus::ERROR) {
284  return "ERROR";
285  }
286  else {
287  return "UNKNOWN";
288  }
289  }
290  else {
291  return "UNKNOWN";
292  }
293  }
294  else {
295  return "UNKNOWN";
296  }
297  }
298 
301  {
302  if (latest_status_) {
303  if (!isStalled()) {
304  if (latest_status_->level == diagnostic_msgs::DiagnosticStatus::OK) {
305  return OK_STATE;
306  }
307  else if (latest_status_->level
308  == diagnostic_msgs::DiagnosticStatus::WARN) {
309  return WARN_STATE;
310  }
311  else if (latest_status_->level
312  == diagnostic_msgs::DiagnosticStatus::ERROR) {
313  return ERROR_STATE;
314  }
315  else {
316  return STALL_STATE;
317  }
318  }
319  else {
320  return STALL_STATE;
321  }
322  }
323  else {
324  return STALL_STATE;
325  }
326  }
327 
329  {
330  QColor ok_color(25, 255, 240, alpha_ * 255.0);
331  QColor warn_color(240, 173, 78, alpha_ * 255.0);
332  QColor error_color(217, 83, 79, alpha_ * 255.0);
333  QColor stall_color(151, 151, 151, alpha_ * 255.0);
334  //QColor fg_color = stall_color;
336  if (state == OK_STATE) {
337  return ok_color;
338  }
339  else if (state == WARN_STATE) {
340  return warn_color;
341  }
342  else if (state == ERROR_STATE) {
343  return error_color;
344  }
345  else {
346  return stall_color;
347  }
348  }
349 
351  {
352  QColor ok_color(40, 40, 40, alpha_ * 255.0);
353  QColor warn_color(255, 255, 255, alpha_ * 255.0);
354  QColor error_color(240, 173, 78, alpha_ * 255.0);
355  QColor stall_color(240, 173, 78, alpha_ * 255.0);
356  //QColor fg_color = stall_color;
358  if (state == OK_STATE) {
359  return ok_color;
360  }
361  else if (state == WARN_STATE) {
362  return warn_color;
363  }
364  else if (state == ERROR_STATE) {
365  return error_color;
366  }
367  else {
368  return stall_color;
369  }
370  }
371 
372 
373 
374  QColor OverlayDiagnosticDisplay::blendColor(QColor a, QColor b, double a_rate) {
375  QColor ret (a.red() * a_rate + b.red() * (1 - a_rate),
376  a.green() * a_rate + b.green() * (1 - a_rate),
377  a.blue() * a_rate + b.blue() * (1 - a_rate),
378  a.alpha() * a_rate + b.alpha() * (1 - a_rate));
379  return ret;
380  }
381 
382  double OverlayDiagnosticDisplay::textWidth(QPainter& painter, double font_size, const std::string& text)
383  {
384  painter.save();
385  const double r = size_ / 128.0;
386  QFont font("Liberation Sans", font_size * r, font_size * r, QFont::Bold);
387  QPen pen;
388  QPainterPath path;
389  pen.setWidth(1);
390  painter.setFont(font);
391  painter.setPen(pen);
392  QFontMetrics metrics(font);
393  const int text_width = metrics.width(text.c_str());
394  const int text_height = metrics.height();
395  painter.restore();
396  return text_width;
397  }
398 
399  double OverlayDiagnosticDisplay::textHeight(QPainter& painter, double font_size)
400  {
401  painter.save();
402  const double r = size_ / 128.0;
403  QFont font("Liberation Sans", font_size * r, font_size * r, QFont::Bold);
404  QPen pen;
405  QPainterPath path;
406  pen.setWidth(1);
407  painter.setFont(font);
408  painter.setPen(pen);
409  QFontMetrics metrics(font);
410  const int text_height = metrics.height();
411  painter.restore();
412  return text_height;
413  }
414 
415 
417  QColor fg_color,
418  const double height,
419  const double font_size,
420  const std::string text)
421  {
422  const double r = size_ / 128.0;
423  QFont font("Liberation Sans", font_size * r, font_size * r, false);
424  QPen pen;
425  QPainterPath path;
426  pen.setWidth(1);
427  pen.setColor(blendColor(fg_color, Qt::white, 0.5));
428  painter.setFont(font);
429  painter.setPen(pen);
430  painter.setBrush(fg_color);
431  QFontMetrics metrics(font);
432  const int text_width = metrics.width(text.c_str());
433  const int text_height = metrics.height();
434  if (overlay_->getTextureWidth() > text_width) {
435  path.addText((overlay_->getTextureWidth() - text_width) / 2.0,
436  height,
437  font, text.c_str());
438  }
439  else {
440  double left = - fmod(t_, overlay_diagnostic_animation_duration) /
441  overlay_diagnostic_animation_duration * text_width * 2 + text_width;
442  path.addText(left, height, font, text.c_str());
443  }
444  painter.drawPath(path);
445  return text_height;
446  }
447 
448  void OverlayDiagnosticDisplay::drawText(QPainter& painter, QColor fg_color,
449  const std::string& text)
450  {
451  double status_size = drawAnimatingText(painter, fg_color,
452  overlay_->getTextureHeight() / 3.0,
453  20, text);
454  double namespace_size = drawAnimatingText(painter, fg_color,
455  overlay_->getTextureHeight() / 3.0 + status_size,
457  std::string message;
458  if (latest_status_) {
459  if (!isStalled()) {
460  message = latest_status_->message;
461  }
462  else {
463  message = "stalled";
464  }
465  }
466  else {
467  message = "stalled";
468  }
469  drawAnimatingText(painter, fg_color,
470  overlay_->getTextureHeight() / 3.0
471  + status_size + namespace_size,
472  10, message);
473  }
474 
476  {
477  QColor fg_color = foregroundColor();
478  // draw outer circle
479  // line-width - margin - inner-line-width < size
480  QPainter painter( &Hud );
481  const int line_width = 10;
482  const int margin = 10;
483  const int inner_line_width = 20;
484 
485  painter.setRenderHint(QPainter::Antialiasing, true);
486  painter.setPen(QPen(fg_color, line_width, Qt::SolidLine));
487  painter.drawEllipse(line_width / 2, line_width / 2,
488  overlay_->getTextureWidth() - line_width,
489  overlay_->getTextureHeight() - line_width);
490 
491  painter.setPen(QPen(fg_color, inner_line_width, Qt::SolidLine));
492  const double start_angle = fmod(t_, overlay_diagnostic_animation_duration) /
493  overlay_diagnostic_animation_duration * 360;
494  const double draw_angle = 250;
495  const double inner_circle_start
496  = line_width + margin + inner_line_width / 2.0;
497  drawText(painter, fg_color, statusText());
498  }
499 
501  QColor color,
502  QColor small_color,
503  int width)
504  {
505  double S = size_;
506  double A = S * 0.1;
507  double B = 0.2 * S;
508  double C = 0.2;
509  painter.setPen(QPen(color, width, Qt::SolidLine));
510  QPainterPath large_rectangle_path;
511  large_rectangle_path.moveTo(A, S - B);
512  large_rectangle_path.lineTo(A, S);
513  large_rectangle_path.lineTo(S, B);
514  large_rectangle_path.lineTo(S, 0);
515  painter.setPen(Qt::NoPen);
516  painter.fillPath(large_rectangle_path, QBrush(color));
517  QPainterPath small_rectangle_path;
518  small_rectangle_path.moveTo(A, S - B);
519  small_rectangle_path.lineTo(A, S - B + C * B);
520  small_rectangle_path.lineTo(A + (S - A) * C, S - B + C * B - (S - B) * C);
521  small_rectangle_path.lineTo(A + (S - A) * C, S - B - (S - B) * C);
522  painter.setPen(Qt::NoPen);
523  painter.fillPath(small_rectangle_path, QBrush(small_color));
524  }
525 
527  QColor color,
528  QColor small_color,
529  int width,
530  double D)
531  {
532  double S = size_;
533  double A = S * 0.1;
534  double B = 0.2 * S;
535  double C = 0.2;
536  //double D = 0.05 * S;
537  painter.setPen(QPen(color, width, Qt::SolidLine));
538  QPainterPath large_rectangle_path;
539  large_rectangle_path.moveTo(A, S - B);
540  large_rectangle_path.lineTo(A, S);
541  double r0 = (S - A - D) / 2 / (S - A);
542  large_rectangle_path.lineTo(A + (S - A - D) / 2, S - r0 * (S - B));
543  large_rectangle_path.lineTo(A + (S - A - D) / 2, S - r0 * (S - B) - B);
544  painter.setPen(Qt::NoPen);
545  painter.fillPath(large_rectangle_path, QBrush(color));
546 
547  QPainterPath large_rectangle_path2;
548  large_rectangle_path2.moveTo(S - (S - A - D) / 2, (S - B) * r0);
549  large_rectangle_path2.lineTo(S - (S - A - D) / 2, (S - B) * r0 + B);
550  large_rectangle_path2.lineTo(S, B);
551  large_rectangle_path2.lineTo(S, 0);
552  painter.setPen(Qt::NoPen);
553  painter.fillPath(large_rectangle_path2, QBrush(color));
554 
555  QPainterPath small_rectangle_path;
556  small_rectangle_path.moveTo(A, S - B);
557  small_rectangle_path.lineTo(A, S - B + C * B);
558  small_rectangle_path.lineTo(A + (S - A) * C, S - B + C * B - (S - B) * C);
559  small_rectangle_path.lineTo(A + (S - A) * C, S - B - (S - B) * C);
560  painter.setPen(Qt::NoPen);
561  painter.fillPath(small_rectangle_path, QBrush(small_color));
562  }
563 
565  {
566  QColor line_color(240, 173, 78, alpha_ * 255.0);
567  QColor rectangle_color = foregroundColor();
568  QColor small_rectangle_color(line_color);
569  QPainter painter(&Hud);
570  State close_state = OK_STATE;
571  int line_width = 2;
572  double S = size_;
573  double A = S * 0.1;
574  double B = 0.2 * S;
575  double C = 0.2;
576  double max_gap = 0.05 * S;
577  painter.setRenderHint(QPainter::Antialiasing, true);
578  painter.setPen(QPen(line_color, line_width, Qt::SolidLine));
579  painter.drawLine(QPoint(0, 0), QPoint(0, S));
580  painter.drawLine(QPoint(0, S - B / 2), QPoint(A, S - B / 2));
581  if (isAnimating()) {
582  // check animation direction
583  double r = animationRate();
584  if (previous_state_ == close_state) { // close -> open
585  drawEVANonConnectedRectangle(painter, rectangle_color, small_rectangle_color, line_width, max_gap * r);
586  }
587  else if (getLatestState() == close_state) { // open -> close
588  drawEVANonConnectedRectangle(painter, rectangle_color, small_rectangle_color, line_width, max_gap * (1 - r));
589  }
590  else { // open -> open
591  drawEVANonConnectedRectangle(painter, rectangle_color, small_rectangle_color, line_width, max_gap);
592  }
593  }
594  else {
596  if (state == close_state) {
597  drawEVAConnectedRectangle(painter, rectangle_color, small_rectangle_color, line_width);
598  }
599  else {
600  drawEVANonConnectedRectangle(painter, rectangle_color, small_rectangle_color, line_width, max_gap);
601  }
602  }
603  painter.setPen(QPen(textColor(), 2 * line_width, Qt::SolidLine));
604  painter.setFont(QFont("Liberation Sans", 12, QFont::Bold));
605  double theta = atan2(S - B, S - A) / M_PI * 180;
606  double text_box_height = cos(theta*M_PI/180) * B;
607  double text_box_width = (S - A) / cos(theta*M_PI/180) - sin(theta*M_PI/180) * B * 2;
608  double text_width = textWidth(painter, 12, diagnostics_namespace_);
609 
610  painter.translate(A, S - B);
611  painter.rotate(-theta);
612  if (text_width > text_box_width) {
613  double text_left = - fmod(t_, overlay_diagnostic_animation_duration) /
614  overlay_diagnostic_animation_duration * text_width;
615  painter.drawText(QRectF(text_left, 0, text_width*2, text_box_height), Qt::AlignLeft | Qt::AlignVCenter | Qt::TextSingleLine,
616  diagnostics_namespace_.c_str());
617  }
618  else {
619  painter.drawText(QRectF(0, 0, text_box_width, text_box_height), Qt::AlignLeft | Qt::AlignVCenter | Qt::TextSingleLine,
620  diagnostics_namespace_.c_str());
621  }
622  }
623 
625  {
626  ScopedPixelBuffer buffer = overlay_->getBuffer();
627  QColor transparent(0, 0, 0, 0.0);
628  QImage Hud = buffer.getQImage(*overlay_, transparent);
629  if (type_ == 0) {
630  drawSAC(Hud);
631  }
632  else if (type_ == 1) {
633  drawEVA(Hud);
634  }
635  }
636 
638  {
639  //QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
641  for (std::set<std::string>::iterator it = namespaces_.begin();
642  it != namespaces_.end();
643  it++) {
645  }
647  }
648 
650  {
651  latest_status_.reset();
652  unsubscribe();
653  subscribe();
654  }
655 
657  {
658  latest_status_.reset();
660  }
661 
663  {
665  }
666 
668  {
670  }
671 
673  {
675  }
676 
678  {
680  }
681 
683  {
685  }
686 
688  {
689  return (top_ < y && top_ + size_ > y &&
690  left_ < x && left_ + size_ > x);
691  }
692 
694  {
695  top_ = y;
696  left_ = x;
697  }
698 
700  {
703  }
704 
706  {
708  }
709 }
710 
virtual double textWidth(QPainter &painter, double font_size, const std::string &text)
void setMin(float min)
virtual bool setValue(const QVariant &new_value)
virtual void processMessage(const diagnostic_msgs::DiagnosticArray::ConstPtr &msg)
void setMax(float max)
virtual double drawAnimatingText(QPainter &painter, QColor fg_color, const double height, const double font_size, const std::string text)
const double overlay_diagnostic_animation_duration
virtual QImage getQImage(unsigned int width, unsigned int height)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
PLUGINLIB_EXPORT_CLASS(jsk_rviz_plugins::PictogramArrayDisplay, rviz::Display)
virtual void clearOptions()
boost::shared_ptr< diagnostic_msgs::DiagnosticStatus > latest_status_
virtual int getInt() const
virtual void update(float wall_dt, float ros_dt)
virtual QColor blendColor(QColor a, QColor b, double a_rate)
virtual float getFloat() const
void setMin(int min)
void addOptionStd(const std::string &option)
virtual void drawEVANonConnectedRectangle(QPainter &painter, QColor color, QColor small_color, int width, double gap)
std::string getStdString()
bool isEnabled() const
virtual double textHeight(QPainter &painter, double font_size)
#define ROS_INFO(...)
void addOptionStd(const std::string &option, int value=0)
virtual void drawEVAConnectedRectangle(QPainter &painter, QColor color, QColor small_color, int width)
static WallTime now()
rviz::EditableEnumProperty * diagnostics_namespace_property_
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
virtual void drawText(QPainter &painter, QColor fg_color, const std::string &text)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
std::string getTopicStd() const
virtual int getOptionInt()
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
const double overlay_diagnostic_animation_transition_duration
#define ROS_DEBUG(...)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sat Mar 20 2021 03:03:18