overlay_diagnostic_display.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "overlay_diagnostic_display.h"
00037 #include <OGRE/OgreOverlayManager.h>
00038 #include <OGRE/OgreMaterialManager.h>
00039 #include <OGRE/OgreTextureManager.h>
00040 #include <OGRE/OgreTexture.h>
00041 #include <OGRE/OgreHardwarePixelBuffer.h>
00042 #include <OGRE/OgreTechnique.h>
00043 
00044 #include <rviz/uniform_string_stream.h>
00045 #include <rviz/display_context.h>
00046 #include <rviz/view_manager.h>
00047 #include <rviz/render_panel.h>
00048 
00049 namespace jsk_rviz_plugins
00050 {
00051   const double overlay_diagnostic_animation_duration = 5.0;
00052   OverlayDiagnosticDisplay::OverlayDiagnosticDisplay()
00053     : Display()
00054   {
00055     ros_topic_property_ = new rviz::RosTopicProperty(
00056       "Topic", "/diagnostics_agg",
00057       ros::message_traits::datatype<diagnostic_msgs::DiagnosticArray>(),
00058       "diagnostic_msgs::DiagnosticArray topic to subscribe to.",
00059       this, SLOT( updateRosTopic() ));
00060     diagnostics_namespace_property_ = new rviz::EditableEnumProperty(
00061       "diagnostics namespace", "/",
00062       "diagnostics namespace to visualize diagnostics",
00063       this, SLOT(updateDiagnosticsNamespace()));
00064     top_property_ = new rviz::IntProperty(
00065       "top", 128,
00066       "top positoin",
00067       this, SLOT(updateTop()));
00068     left_property_ = new rviz::IntProperty(
00069       "left", 128,
00070       "left positoin",
00071       this, SLOT(updateLeft()));
00072     size_property_ = new rviz::IntProperty(
00073       "size", 128,
00074       "size of the widget",
00075       this, SLOT(updateSize()));
00076     size_property_->setMin(1);
00077     alpha_property_ = new rviz::FloatProperty(
00078       "alpha", 0.8,
00079       "alpha value",
00080       this, SLOT(updateAlpha()));
00081     alpha_property_->setMin(0.0);
00082     alpha_property_->setMax(1.0);
00083     stall_duration_property_ = new rviz::FloatProperty(
00084       "stall duration", 5.0,
00085       "seconds to be regarded as stalled",
00086       this, SLOT(updateStallDuration())
00087       );
00088     stall_duration_property_->setMin(0.0);
00089   }
00090 
00091   OverlayDiagnosticDisplay::~OverlayDiagnosticDisplay()
00092   {
00093     if (overlay_) {
00094       overlay_->hide();
00095     }
00096     // panel_material_->unload();
00097     // Ogre::MaterialManager::getSingleton().remove(panel_material_->getName());
00098     delete ros_topic_property_;
00099     delete diagnostics_namespace_property_;
00100     delete top_property_;
00101     delete left_property_;
00102     delete alpha_property_;
00103     delete size_property_;
00104   }
00105 
00106   void OverlayDiagnosticDisplay::processMessage(
00107     const diagnostic_msgs::DiagnosticArray::ConstPtr& msg)
00108   {
00109     boost::mutex::scoped_lock(mutex_);
00110     //boost::make_shared<diagnostic_msgs::DiagnosticStatus>
00111     // update namespaces_ if needed
00112     std::set<std::string> new_namespaces;
00113     for (size_t i = 0; i < msg->status.size(); i++) {
00114       new_namespaces.insert(msg->status[i].name);
00115     }
00116     
00117     std::set<std::string> difference_namespaces;
00118     std::set_difference(namespaces_.begin(), namespaces_.end(),
00119                         new_namespaces.begin(), new_namespaces.end(),
00120                         std::inserter(difference_namespaces,
00121                                       difference_namespaces.end()));
00122     if (difference_namespaces.size() != 0) {
00123       namespaces_ = new_namespaces;
00124       fillNamespaceList();
00125     }
00126     else {
00127       difference_namespaces.clear();
00128       std::set_difference(new_namespaces.begin(), new_namespaces.end(),
00129                           namespaces_.begin(), namespaces_.end(),
00130                           std::inserter(difference_namespaces,
00131                                         difference_namespaces.end()));
00132       if (difference_namespaces.size() != 0) {
00133         namespaces_ = new_namespaces;
00134         fillNamespaceList();
00135       }
00136     }
00137     
00138     if (diagnostics_namespace_.length() == 0) {
00139       return;
00140     }
00141 
00142     for (size_t i = 0; i < msg->status.size(); i++) {
00143       diagnostic_msgs::DiagnosticStatus status = msg->status[i];
00144       if (status.name == diagnostics_namespace_) {
00145         latest_status_
00146           = boost::make_shared<diagnostic_msgs::DiagnosticStatus>(status);
00147         latest_message_time_ = ros::WallTime::now();
00148         break;
00149       }
00150     }
00151   }
00152 
00153   void OverlayDiagnosticDisplay::update(float wall_dt, float ros_dt)
00154   {
00155     boost::mutex::scoped_lock(mutex_);
00156     if (!isEnabled()) {
00157       return;
00158     }
00159     if (!overlay_) {
00160       static int count = 0;
00161       rviz::UniformStringStream ss;
00162       ss << "OverlayDiagnosticDisplayObject" << count++;
00163       overlay_.reset(new OverlayObject(ss.str()));
00164       overlay_->show();
00165     }
00166     t_ += wall_dt;
00167     
00168     overlay_->updateTextureSize(size_, size_);
00169     redraw();
00170     overlay_->setDimensions(overlay_->getTextureWidth(),
00171                             overlay_->getTextureHeight());
00172     overlay_->setPosition(left_, top_);
00173     t_ = fmod(t_, overlay_diagnostic_animation_duration);
00174   }
00175 
00176   void OverlayDiagnosticDisplay::onEnable()
00177   {
00178     t_ = 0.0;
00179     if (overlay_) {
00180       overlay_->show();
00181     }
00182     subscribe();
00183   }
00184 
00185   void OverlayDiagnosticDisplay::onDisable()
00186   {
00187     ROS_INFO("onDisable");
00188     if (overlay_) {
00189       overlay_->hide();
00190     }
00191     unsubscribe();
00192   }
00193 
00194   void OverlayDiagnosticDisplay::onInitialize()
00195   {
00196     
00197     ROS_DEBUG("onInitialize");
00198 
00199 
00200     updateDiagnosticsNamespace();
00201     updateSize();
00202     updateAlpha();
00203     updateLeft();
00204     updateTop();
00205     updateStallDuration();
00206     updateRosTopic();
00207   }
00208   
00209   void OverlayDiagnosticDisplay::unsubscribe()
00210   {
00211     sub_.shutdown();
00212   }
00213   
00214   void OverlayDiagnosticDisplay::subscribe()
00215   {
00216     ros::NodeHandle n;
00217     sub_ = n.subscribe(ros_topic_property_->getTopicStd(),
00218                        1,
00219                        &OverlayDiagnosticDisplay::processMessage,
00220                        this);
00221   }
00222 
00223   bool OverlayDiagnosticDisplay::isStalled()
00224   {
00225     if (latest_status_) {
00226       ros::WallDuration message_duration
00227         = ros::WallTime::now() - latest_message_time_;
00228       if (message_duration.toSec() < stall_duration_) {
00229         return false;
00230       }
00231       else {
00232         return true;
00233       }
00234     }
00235     else {
00236       return true;
00237     }
00238   }
00239   
00240   std::string OverlayDiagnosticDisplay::statusText()
00241   {
00242     if (latest_status_) {
00243       if (!isStalled()) {
00244         if (latest_status_->level == diagnostic_msgs::DiagnosticStatus::OK) {
00245           return "OK";
00246         }
00247         else if (latest_status_->level == diagnostic_msgs::DiagnosticStatus::WARN) {
00248           return "WARN";
00249         }
00250         else if (latest_status_->level == diagnostic_msgs::DiagnosticStatus::ERROR) {
00251           return "ERROR";
00252         }
00253         else {
00254           return "UNKNOWN";
00255         }
00256       }
00257       else {
00258         return "UNKNOWN";
00259       }
00260     }
00261     else {
00262       return "UNKNOWN";
00263     }
00264   }
00265   
00266   
00267   QColor OverlayDiagnosticDisplay::foregroundColor()
00268   {
00269     QColor ok_color(25, 255, 240, alpha_ * 255.0);
00270     QColor warn_color(240, 173, 78, alpha_ * 255.0);
00271     QColor error_color(217, 83, 79, alpha_ * 255.0);
00272     QColor stall_color(151, 151, 151, alpha_ * 255.0);
00273     //QColor fg_color = stall_color;
00274     
00275     if (latest_status_) {
00276       if (!isStalled()) {
00277         if (latest_status_->level == diagnostic_msgs::DiagnosticStatus::OK) {
00278           return ok_color;
00279         }
00280         else if (latest_status_->level
00281                  == diagnostic_msgs::DiagnosticStatus::WARN) {
00282           return warn_color;
00283         }
00284         else if (latest_status_->level
00285                  == diagnostic_msgs::DiagnosticStatus::ERROR) {
00286           return error_color;
00287         }
00288         else {
00289           return stall_color;
00290         }
00291       }
00292       else {
00293         return stall_color;
00294       }
00295     }
00296     else {
00297       return stall_color;
00298     }
00299   }
00300 
00301   QColor OverlayDiagnosticDisplay::blendColor(QColor a, QColor b, double a_rate) {
00302     QColor ret (a.red() * a_rate + b.red() * (1 - a_rate),
00303                 a.green() * a_rate + b.green() * (1 - a_rate),
00304                 a.blue() * a_rate + b.blue() * (1 - a_rate),
00305                 a.alpha() * a_rate + b.alpha() * (1 - a_rate));
00306     return ret;
00307   }
00308 
00309   double OverlayDiagnosticDisplay::drawAnimatingText(QPainter& painter,
00310                                                      QColor fg_color,
00311                                                      const double height,
00312                                                      const double font_size,
00313                                                      const std::string text)
00314   {
00315     const double r = size_ / 128.0;
00316     QFont font("Arial", font_size * r, font_size * r, false);
00317     QPen pen;
00318     QPainterPath path;
00319     pen.setWidth(1);
00320     pen.setColor(blendColor(fg_color, Qt::white, 0.5));
00321     painter.setFont(font);
00322     painter.setPen(pen);
00323     painter.setBrush(fg_color);
00324     QFontMetrics metrics(font);
00325     const int text_width = metrics.width(text.c_str());
00326     const int text_height = metrics.height();
00327     if (overlay_->getTextureWidth() > text_width) {
00328       path.addText((overlay_->getTextureWidth() - text_width) / 2.0,
00329                    height,
00330                    font, text.c_str());
00331     }
00332     else {
00333       double left = - fmod(t_, overlay_diagnostic_animation_duration) /
00334         overlay_diagnostic_animation_duration * text_width * 2 + text_width;
00335       path.addText(left, height, font, text.c_str());
00336     }
00337     painter.drawPath(path);
00338     return text_height;
00339   }
00340   
00341   void OverlayDiagnosticDisplay::drawText(QPainter& painter, QColor fg_color,
00342                                           const std::string& text)
00343   {
00344     double status_size = drawAnimatingText(painter, fg_color,
00345                                            overlay_->getTextureHeight() / 3.0,
00346                                            20, text);
00347     double namespace_size = drawAnimatingText(painter, fg_color,
00348                                               overlay_->getTextureHeight() / 3.0 + status_size,
00349                                               10, diagnostics_namespace_);
00350     std::string message;
00351     if (latest_status_) {
00352       if (!isStalled()) {
00353         message = latest_status_->message;
00354       }
00355       else {
00356         message = "stalled";
00357       }
00358     }
00359     else {
00360       message = "stalled";
00361     }
00362     drawAnimatingText(painter, fg_color,
00363                       overlay_->getTextureHeight() / 3.0
00364                       + status_size + namespace_size,
00365                       10, message);
00366   }
00367   
00368   void OverlayDiagnosticDisplay::redraw()
00369   {
00370     ScopedPixelBuffer buffer = overlay_->getBuffer();
00371     QColor fg_color = foregroundColor();
00372     QColor transparent(0, 0, 0, 0.0);
00373     
00374     QImage Hud = buffer.getQImage(*overlay_, transparent);
00375     // draw outer circle
00376     // line-width - margin - inner-line-width < size
00377     QPainter painter( &Hud );
00378     const int line_width = 10;
00379     const int margin = 10;
00380     const int inner_line_width = 20;
00381     
00382     painter.setRenderHint(QPainter::Antialiasing, true);
00383     painter.setPen(QPen(fg_color, line_width, Qt::SolidLine));
00384     painter.drawEllipse(line_width / 2, line_width / 2,
00385                         overlay_->getTextureWidth() - line_width,
00386                         overlay_->getTextureHeight() - line_width);
00387     
00388     painter.setPen(QPen(fg_color, inner_line_width, Qt::SolidLine));    
00389     const double start_angle = fmod(t_, overlay_diagnostic_animation_duration) /
00390       overlay_diagnostic_animation_duration * 360;
00391     const double draw_angle = 250;
00392     const double inner_circle_start
00393       = line_width + margin + inner_line_width / 2.0;
00394     drawText(painter, fg_color, statusText());
00395   }
00396 
00397   void OverlayDiagnosticDisplay::fillNamespaceList()
00398   {
00399     //QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
00400     diagnostics_namespace_property_->clearOptions();
00401     for (std::set<std::string>::iterator it = namespaces_.begin();
00402          it != namespaces_.end();
00403          it++) {
00404       diagnostics_namespace_property_->addOptionStd(*it);
00405     }
00406     diagnostics_namespace_property_->sortOptions();
00407   }
00408   
00409   void OverlayDiagnosticDisplay::updateRosTopic()
00410   {
00411     latest_status_.reset();
00412     unsubscribe();
00413     subscribe();
00414   }
00415 
00416   void OverlayDiagnosticDisplay::updateDiagnosticsNamespace()
00417   {
00418     latest_status_.reset();
00419     diagnostics_namespace_ = diagnostics_namespace_property_->getStdString();
00420   }
00421   
00422   void OverlayDiagnosticDisplay::updateSize()
00423   {
00424     size_ = size_property_->getInt();
00425   }
00426 
00427   void OverlayDiagnosticDisplay::updateAlpha()
00428   {
00429     alpha_ = alpha_property_->getFloat();
00430   }
00431 
00432   void OverlayDiagnosticDisplay::updateTop()
00433   {
00434     top_ = top_property_->getInt();
00435   }
00436   
00437   void OverlayDiagnosticDisplay::updateLeft()
00438   {
00439     left_ = left_property_->getInt();
00440   }
00441   
00442   void OverlayDiagnosticDisplay::updateStallDuration()
00443   {
00444     stall_duration_ = stall_duration_property_->getFloat();
00445   }
00446 }
00447 
00448 #include <pluginlib/class_list_macros.h>
00449 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::OverlayDiagnosticDisplay, rviz::Display)


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03