diagnostics_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 "diagnostics_display.h"
00037 #include <boost/algorithm/string/predicate.hpp>
00038 #include <boost/lexical_cast.hpp>
00039 
00040 namespace jsk_rviz_plugins
00041 {
00042 
00043   DiagnosticsDisplay::DiagnosticsDisplay()
00044     : rviz::Display(), msg_(0)
00045   {
00046     ros_topic_property_
00047       = new rviz::RosTopicProperty(
00048         "Topic", "/diagnostics_agg",
00049         ros::message_traits::datatype<diagnostic_msgs::DiagnosticArray>(),
00050         "diagnostic_msgs::DiagnosticArray topic to subscribe to.",
00051         this, SLOT( updateRosTopic() ));
00052     frame_id_property_ = new rviz::TfFrameProperty(
00053       "frame_id", rviz::TfFrameProperty::FIXED_FRAME_STRING,
00054       "the parent frame_id to visualize diagnostics",
00055       this, 0, true);
00056     diagnostics_namespace_property_ = new rviz::EditableEnumProperty(
00057       "diagnostics namespace", "/",
00058       "diagnostics namespace to visualize diagnostics",
00059       this, SLOT(updateDiagnosticsNamespace()));
00060     radius_property_ = new rviz::FloatProperty(
00061       "radius", 1.0,
00062       "radius of diagnostics circle",
00063       this, SLOT(updateRadius()));
00064     line_width_property_ = new rviz::FloatProperty(
00065       "line width", 0.03,
00066       "line width",
00067       this, SLOT(updateLineWidth()));
00068     axis_property_ = new rviz::EnumProperty(
00069       "axis", "x",
00070       "axis",
00071       this, SLOT(updateAxis()));
00072     axis_property_->addOption("x", 0);
00073     axis_property_->addOption("y", 1);
00074     axis_property_->addOption("z", 2);
00075     font_size_property_ = new rviz::FloatProperty(
00076       "font size", 0.05,
00077       "font size",
00078       this, SLOT(updateFontSize()));
00079   }
00080 
00081   DiagnosticsDisplay::~DiagnosticsDisplay()
00082   {
00083     delete ros_topic_property_;
00084     delete frame_id_property_;
00085     delete diagnostics_namespace_property_;
00086     delete radius_property_;
00087     delete line_width_property_;
00088     delete axis_property_;
00089     delete line_;
00090     delete msg_;
00091     delete font_size_property_;
00092   }
00093 
00094   void DiagnosticsDisplay::update(float wall_dt, float ros_dt)
00095   {
00096     if (line_update_required_) {
00097       updateLine();
00098     }
00099 
00100     if (!isEnabled()) {
00101       return;
00102     }
00103 
00104     msg_->setCharacterHeight(font_size_);
00105     
00106     const float round_trip = 10.0;
00107     Ogre::Quaternion orientation;
00108     Ogre::Vector3 position;
00109     std::string frame_id = frame_id_property_->getFrame().toStdString();
00110     if( !context_->getFrameManager()->getTransform( frame_id,
00111                                                     ros::Time(0.0),
00112                                                     position, orientation ))
00113     {
00114       ROS_WARN( "Error transforming from frame '%s' to frame '%s'",
00115                 frame_id.c_str(), qPrintable( fixed_frame_ ));
00116       return;
00117     }
00118     scene_node_->setPosition(position);
00119     scene_node_->setOrientation(orientation);
00120     Ogre::Vector3 orbit_position;
00121     orbit_theta_ = ros_dt / round_trip * M_PI * 2.0 + orbit_theta_;
00122     while (orbit_theta_ > M_PI * 2) {
00123       orbit_theta_ -= M_PI*2;
00124     }
00125     if (axis_ == 0) {           // x
00126       orbit_position.x = radius_ * cos(orbit_theta_);
00127       orbit_position.y = radius_ * sin(orbit_theta_);
00128       orbit_position.z = 0;
00129     }
00130     else if (axis_ == 1) {      // y
00131       orbit_position.y = radius_ * cos(orbit_theta_);
00132       orbit_position.z = radius_ * sin(orbit_theta_);
00133       orbit_position.x = 0;
00134     }
00135     else if (axis_ == 2) {      // z
00136       orbit_position.z = radius_ * cos(orbit_theta_);
00137       orbit_position.x = radius_ * sin(orbit_theta_);
00138       orbit_position.y = 0;
00139     }
00140     
00141     orbit_node_->setPosition(orbit_position);
00142     if (!isEnabled()) {
00143       return;
00144     }
00145     context_->queueRender();
00146   }
00147   
00148   void DiagnosticsDisplay::onInitialize()
00149   {
00150     static int counter = 0;
00151     scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00152     orbit_node_ = scene_node_->createChildSceneNode(); // ??
00153     line_ = new rviz::BillboardLine(context_->getSceneManager(), scene_node_);
00154     msg_ = new rviz::MovableText("not initialized", "Liberation Sans", 0.05);
00155     msg_->setTextAlignment(rviz::MovableText::H_CENTER,
00156                            rviz::MovableText::V_ABOVE);
00157     frame_id_property_->setFrameManager(context_->getFrameManager());
00158     orbit_node_->attachObject(msg_);
00159     msg_->setVisible(false);
00160     orbit_theta_ = M_PI * 2.0 / 6 * counter++;
00161     updateLineWidth();
00162     updateAxis();
00163     updateDiagnosticsNamespace();
00164     updateRadius();
00165     updateRosTopic();
00166     updateFontSize();
00167   }
00168   
00169   void DiagnosticsDisplay::processMessage
00170   (const diagnostic_msgs::DiagnosticArray::ConstPtr& msg)
00171   {
00172     if (!isEnabled()) {
00173       return;
00174     }
00175 
00176     // update namespaces_ if needed
00177     std::set<std::string> new_namespaces;
00178     for (size_t i = 0; i < msg->status.size(); i++) {
00179       new_namespaces.insert(msg->status[i].name);
00180     }
00181     
00182     std::set<std::string> difference_namespaces;
00183     std::set_difference(namespaces_.begin(), namespaces_.end(),
00184                         new_namespaces.begin(), new_namespaces.end(),
00185                         std::inserter(difference_namespaces,
00186                                       difference_namespaces.end()));
00187     if (difference_namespaces.size() != 0) {
00188       namespaces_ = new_namespaces;
00189       fillNamespaceList();
00190     }
00191     else {
00192       difference_namespaces.clear();
00193       std::set_difference(new_namespaces.begin(), new_namespaces.end(),
00194                           namespaces_.begin(), namespaces_.end(),
00195                           std::inserter(difference_namespaces,
00196                                         difference_namespaces.end()));
00197       if (difference_namespaces.size() != 0) {
00198         namespaces_ = new_namespaces;
00199         fillNamespaceList();
00200       }
00201     }
00202     
00203     if (diagnostics_namespace_.length() == 0) {
00204       return;
00205     }
00206     
00207     const float alpha = 0.8;
00208     const Ogre::ColourValue OK(0.3568627450980392, 0.7529411764705882, 0.8705882352941177, alpha);
00209     const Ogre::ColourValue WARN(0.9411764705882353, 0.6784313725490196, 0.3058823529411765, alpha);
00210     const Ogre::ColourValue ERROR(0.8509803921568627, 0.3254901960784314, 0.30980392156862746, 0.5);
00211     const Ogre::ColourValue UNKNOWN(0.2, 0.2, 0.2, 0.5);
00212     Ogre::ColourValue color;
00213     std::string message;
00214     bool foundp = false;
00215     for (size_t i = 0; i < msg->status.size(); i++) {
00216       diagnostic_msgs::DiagnosticStatus status = msg->status[i];
00217       if (status.name == diagnostics_namespace_) {
00218         if (status.level == diagnostic_msgs::DiagnosticStatus::OK) {
00219           color = OK;
00220           message = status.message;
00221         }
00222         else if (status.level == diagnostic_msgs::DiagnosticStatus::WARN) {
00223           color = WARN;
00224           message = status.message;
00225         }
00226         else if (status.level == diagnostic_msgs::DiagnosticStatus::ERROR) {
00227           color = ERROR;
00228           message = status.message;
00229         }
00230         else {
00231           // unknwon
00232           color = UNKNOWN;
00233           message = "unknown";
00234         }
00235         foundp = true;
00236         break;
00237       }
00238     }
00239 
00240     if (!foundp) {
00241       color = UNKNOWN;
00242       message = "stall";
00243     }
00244     
00245     line_->setColor(color.r, color.g, color.b, color.a);
00246     Ogre::ColourValue font_color(color);
00247     font_color.a = 1.0;
00248     msg_->setColor(font_color);
00249     msg_->setCaption(diagnostics_namespace_ + "\n" + message);
00250     context_->queueRender();
00251   }
00252 
00253   void DiagnosticsDisplay::updateLine()
00254   {
00255     line_->clear();
00256     line_->setLineWidth(line_width_);
00257     line_->setNumLines(1);
00258     line_->setMaxPointsPerLine(1024);
00259     for (size_t i = 0; i < 128 + 1; i++) {
00260       Ogre::Vector3 step_position;
00261       const double theta = M_PI * 2.0 / 128 * i;
00262       if (axis_ == 0) {
00263         step_position.x = radius_ * cos(theta);
00264         step_position.y = radius_ * sin(theta);
00265         step_position.z = 0.0;
00266       }
00267       else if (axis_ == 1) {
00268         step_position.y = radius_ * cos(theta);
00269         step_position.z = radius_ * sin(theta);
00270         step_position.x = 0;
00271       }
00272       else if (axis_ == 2) {
00273         step_position.z = radius_ * cos(theta);
00274         step_position.x = radius_ * sin(theta);
00275         step_position.y = 0;
00276       }
00277       line_->addPoint(step_position);
00278     }
00279     line_update_required_ = false;
00280   }
00281   
00282   void DiagnosticsDisplay::onEnable()
00283   {
00284     line_update_required_ = true;
00285     msg_->setVisible(true);
00286   }
00287 
00288   void DiagnosticsDisplay::onDisable()
00289   {
00290     unsubscribe();
00291     line_->clear();
00292     msg_->setVisible(false);
00293   }
00294   
00295   void DiagnosticsDisplay::unsubscribe()
00296   {
00297     sub_.shutdown();
00298   }
00299   
00300   void DiagnosticsDisplay::subscribe()
00301   {
00302     ros::NodeHandle n;
00303     sub_ = n.subscribe(ros_topic_property_->getTopicStd(),
00304                        1,
00305                        &DiagnosticsDisplay::processMessage,
00306                        this);
00307   }
00308   
00309   void DiagnosticsDisplay::updateRosTopic()
00310   {
00311     unsubscribe();
00312     subscribe();
00313   }
00314 
00315   void DiagnosticsDisplay::updateDiagnosticsNamespace()
00316   {
00317     diagnostics_namespace_ = diagnostics_namespace_property_->getStdString();
00318   }
00319   
00320   void DiagnosticsDisplay::updateRadius()
00321   {
00322     radius_ = radius_property_->getFloat();
00323     line_update_required_ = true;
00324   }
00325 
00326   void DiagnosticsDisplay::updateLineWidth()
00327   {
00328     line_width_ = line_width_property_->getFloat();
00329     line_update_required_ = true;
00330   }
00331 
00332 
00333   void DiagnosticsDisplay::updateAxis()
00334   {
00335     axis_ = axis_property_->getOptionInt();
00336     line_update_required_ = true;
00337   }
00338 
00339   void DiagnosticsDisplay::fillNamespaceList()
00340   {
00341     //QApplication::setOverrideCursor(QCursor(Qt::WaitCursor));
00342     diagnostics_namespace_property_->clearOptions();
00343     for (std::set<std::string>::iterator it = namespaces_.begin();
00344          it != namespaces_.end();
00345          it++) {
00346       diagnostics_namespace_property_->addOptionStd(*it);
00347     }
00348     diagnostics_namespace_property_->sortOptions();
00349   }
00350 
00351   void DiagnosticsDisplay::updateFontSize()
00352   {
00353     font_size_ = font_size_property_->getFloat();
00354   }
00355   
00356 }
00357 
00358 #include <pluginlib/class_list_macros.h>
00359 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::DiagnosticsDisplay, rviz::Display )


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Wed May 1 2019 02:40:22