plotter_2d_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 "plotter_2d_display.h"
00037 #include <OGRE/OgreHardwarePixelBuffer.h>
00038 #include <rviz/uniform_string_stream.h>
00039 #include <rviz/display_context.h>
00040 #include <QPainter>
00041 
00042 namespace jsk_rviz_plugins
00043 {
00044   Plotter2DDisplay::Plotter2DDisplay()
00045     : rviz::Display(), min_value_(0.0), max_value_(0.0)
00046   {
00047     update_topic_property_ = new rviz::RosTopicProperty(
00048       "Topic", "",
00049       ros::message_traits::datatype<std_msgs::Float32>(),
00050       "std_msgs::Float32 topic to subscribe to.",
00051       this, SLOT(updateTopic()));
00052     show_value_property_ = new rviz::BoolProperty(
00053       "Show Value", true,
00054       "Show value on plotter",
00055       this, SLOT(updateShowValue()));
00056     buffer_length_property_ = new rviz::IntProperty(
00057       "Buffer length", 100,
00058       ros::message_traits::datatype<std_msgs::Float32>(),
00059       this, SLOT(updateBufferSize()));
00060     width_property_ = new rviz::IntProperty("width", 128,
00061                                             "width of the plotter window",
00062                                             this, SLOT(updateWidth()));
00063     width_property_->setMin(1);
00064     width_property_->setMax(2000);
00065     height_property_ = new rviz::IntProperty("height", 128,
00066                                              "height of the plotter window",
00067                                              this, SLOT(updateHeight()));
00068     height_property_->setMin(1);
00069     height_property_->setMax(2000);
00070     left_property_ = new rviz::IntProperty("left", 128,
00071                                            "left of the plotter window",
00072                                            this, SLOT(updateLeft()));
00073     left_property_->setMin(0);
00074     top_property_ = new rviz::IntProperty("top", 128,
00075                                           "top of the plotter window",
00076                                           this, SLOT(updateTop()));
00077     top_property_->setMin(0);
00078     auto_scale_property_ = new rviz::BoolProperty(
00079       "auto scale", true,
00080       "enable auto scale",
00081       this, SLOT(updateAutoScale()));
00082     max_value_property_ = new rviz::FloatProperty(
00083       "max value", 1.0,
00084       "max value, used only if auto scale is disabled",
00085       this, SLOT(updateMaxValue()));
00086     min_value_property_ = new rviz::FloatProperty(
00087       "min value", -1.0,
00088       "min value, used only if auto scale is disabled",
00089       this, SLOT(updateMinValue()));
00090     fg_color_property_ = new rviz::ColorProperty(
00091       "foreground color", QColor(25, 255, 240),
00092       "color to draw line",
00093       this, SLOT(updateFGColor()));
00094     fg_alpha_property_ = new rviz::FloatProperty(
00095       "foreground alpha", 0.7,
00096       "alpha belnding value for foreground",
00097       this, SLOT(updateFGAlpha()));
00098     fg_alpha_property_->setMin(0);
00099     fg_alpha_property_->setMax(1.0);
00100     bg_color_property_ = new rviz::ColorProperty(
00101       "background color", QColor(0, 0, 0),
00102       "background color",
00103       this, SLOT(updateBGColor()));
00104     bg_alpha_property_ = new rviz::FloatProperty(
00105       "backround alpha", 0.0,
00106       "alpha belnding value for background",
00107       this, SLOT(updateBGAlpha()));
00108     bg_alpha_property_->setMin(0);
00109     bg_alpha_property_->setMax(1.0);
00110     line_width_property_ = new rviz::IntProperty("linewidth", 1,
00111                                                  "linewidth of the plot",
00112                                                  this, SLOT(updateLineWidth()));
00113     line_width_property_->setMin(1);
00114     line_width_property_->setMax(1000);
00115     show_border_property_ = new rviz::BoolProperty(
00116       "border", true,
00117       "show border or not",
00118       this, SLOT(updateShowBorder()));
00119     text_size_property_ = new rviz::IntProperty("text size", 12,
00120                                                 "text size of the caption",
00121                                                 this, SLOT(updateTextSize()));
00122     text_size_property_->setMin(1);
00123     text_size_property_->setMax(1000);
00124     show_caption_property_ = new rviz::BoolProperty(
00125       "caption", true,
00126       "show caption or not",
00127       this, SLOT(updateShowCaption()));
00128     update_interval_property_ = new rviz::FloatProperty(
00129       "update interval", 0.04,
00130       "update interval of the plotter",
00131       this, SLOT(updateUpdateInterval()));
00132     update_interval_property_->setMin(0.0);
00133     update_interval_property_->setMax(100);
00134     auto_color_change_property_
00135       = new rviz::BoolProperty("auto color change",
00136                                false,
00137                                "change the color automatically",
00138                                this, SLOT(updateAutoColorChange()));
00139     max_color_property_
00140       = new rviz::ColorProperty(
00141         "max color",
00142         QColor(255, 0, 0),
00143         "only used if auto color change is set to True.",
00144         this, SLOT(updateMaxColor()));
00145   }
00146 
00147   Plotter2DDisplay::~Plotter2DDisplay()
00148   {
00149     onDisable();
00150     // delete update_topic_property_;
00151     // delete buffer_length_property_;
00152     // delete fg_color_property_;
00153     // delete bg_color_property_;
00154     // delete fg_alpha_property_;
00155     // delete bg_alpha_property_;
00156     // delete top_property_;
00157     // delete left_property_;
00158     // delete width_property_;
00159     // delete height_property_;
00160     // delete line_width_property_;
00161     // delete show_border_property_;
00162     // delete auto_color_change_property_;
00163     // delete max_color_property_;
00164     // delete update_interval_property_;
00165     // delete show_caption_property_;
00166     // delete text_size_property_;
00167     // delete min_value_property_;
00168     // delete max_value_property_;
00169     // delete auto_color_change_property_;
00170   }
00171 
00172   void Plotter2DDisplay::initializeBuffer()
00173   {
00174     buffer_.resize(buffer_length_);
00175     if (min_value_ == 0.0 && max_value_ == 0.0) {
00176       min_value_ = -1.0;
00177       max_value_ = 1.0;
00178     }
00179     for (size_t i = 0; i < buffer_length_; i++) {
00180       buffer_[i] = 0.0;
00181     }
00182   }
00183 
00184   void Plotter2DDisplay::onInitialize()
00185   {
00186     static int count = 0;
00187     rviz::UniformStringStream ss;
00188     ss << "Plotter2DDisplayObject" << count++;
00189     overlay_.reset(new OverlayObject(ss.str()));
00190     updateBufferSize();
00191     onEnable();
00192     updateShowValue();
00193     updateWidth();
00194     updateHeight();
00195     updateLeft();
00196     updateTop();
00197     updateFGColor();
00198     updateBGColor();
00199     updateFGAlpha();
00200     updateBGAlpha();
00201     updateLineWidth();
00202     updateUpdateInterval();
00203     updateShowBorder();
00204     updateAutoColorChange();
00205     updateMaxColor();
00206     updateShowCaption();
00207     updateTextSize();
00208     updateAutoScale();
00209     updateMinValue();
00210     updateMaxValue();
00211     overlay_->updateTextureSize(width_property_->getInt(),
00212                                 height_property_->getInt() + caption_offset_);
00213   }
00214 
00215   void Plotter2DDisplay::drawPlot()
00216   {
00217     QColor fg_color(fg_color_);
00218     QColor bg_color(bg_color_);
00219     
00220     fg_color.setAlpha(fg_alpha_);
00221     bg_color.setAlpha(bg_alpha_);
00222 
00223     if (auto_color_change_) {
00224       double r
00225         = std::min(std::max((buffer_[buffer_.size() - 1] - min_value_) / (max_value_ - min_value_),
00226                             0.0), 1.0);
00227       if (r > 0.3) {
00228         double r2 = (r - 0.3) / 0.7;
00229         fg_color.setRed((max_color_.red() - fg_color_.red()) * r2
00230                         + fg_color_.red());
00231         fg_color.setGreen((max_color_.green() - fg_color_.green()) * r2
00232                           + fg_color_.green());
00233         fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r2
00234                          + fg_color_.blue());
00235       }
00236     }
00237     
00238     {
00239       ScopedPixelBuffer buffer = overlay_->getBuffer();
00240       QImage Hud = buffer.getQImage(*overlay_);
00241       // initilize by the background color
00242       for (int i = 0; i < overlay_->getTextureWidth(); i++) {
00243         for (int j = 0; j < overlay_->getTextureHeight(); j++) {
00244           Hud.setPixel(i, j, bg_color.rgba());
00245         }
00246       }
00247       // paste in HUD speedometer. I resize the image and offset it by 8 pixels from
00248       // the bottom left edge of the render window
00249       QPainter painter( &Hud );
00250       painter.setRenderHint(QPainter::Antialiasing, true);
00251       painter.setPen(QPen(fg_color, line_width_, Qt::SolidLine));
00252       
00253       uint16_t w = overlay_->getTextureWidth();
00254       uint16_t h = overlay_->getTextureHeight() - caption_offset_;
00255 
00256       double margined_max_value = max_value_ + (max_value_ - min_value_) / 2;
00257       double margined_min_value = min_value_ - (max_value_ - min_value_) / 2;
00258       
00259       for (size_t i = 1; i < buffer_length_; i++) {
00260         double v_prev = (margined_max_value - buffer_[i - 1]) / (margined_max_value - margined_min_value);
00261         double v = (margined_max_value - buffer_[i]) / (margined_max_value - margined_min_value);
00262         double u_prev = (i - 1) / (float)buffer_length_;
00263         double u = i / (float)buffer_length_;
00264 
00265         // chop within 0 ~ 1
00266         v_prev = std::max(std::min(v_prev, 1.0), 0.0);
00267         u_prev = std::max(std::min(u_prev, 1.0), 0.0);
00268         v = std::max(std::min(v, 1.0), 0.0);
00269         u = std::max(std::min(u, 1.0), 0.0);
00270         
00271         uint16_t x_prev = (int)(u_prev * w);
00272         uint16_t x = (int)(u * w);
00273         uint16_t y_prev = (int)(v_prev * h);
00274         uint16_t y = (int)(v * h);
00275         painter.drawLine(x_prev, y_prev, x, y);
00276       }
00277       // draw border
00278       if (show_border_) {
00279         painter.drawLine(0, 0, 0, h);
00280         painter.drawLine(0, h, w, h);
00281         painter.drawLine(w, h, w, 0);
00282         painter.drawLine(w, 0, 0, 0);
00283       }
00284       // draw caption
00285       if (show_caption_) {
00286         QFont font = painter.font();
00287         font.setPointSize(text_size_);
00288         font.setBold(true);
00289         painter.setFont(font);
00290         painter.drawText(0, h, w, caption_offset_,
00291                          Qt::AlignCenter | Qt::AlignVCenter,
00292                          getName());
00293       }
00294       if (show_value_) {
00295         QFont font = painter.font();
00296         font.setPointSize(w / 4);
00297         font.setBold(true);
00298         painter.setFont(font);
00299         std::ostringstream ss;
00300         ss << std::fixed << std::setprecision(2) << buffer_[buffer_.size() - 1];
00301         painter.drawText(0, 0, w, h,
00302                          Qt::AlignCenter | Qt::AlignVCenter,
00303                          ss.str().c_str());
00304       }
00305       
00306       // done
00307       painter.end();
00308     }
00309   }
00310   
00311   void Plotter2DDisplay::processMessage(const std_msgs::Float32::ConstPtr& msg)
00312   {
00313     boost::mutex::scoped_lock lock(mutex_);
00314 
00315     if (!isEnabled()) {
00316       return;
00317     }
00318     // add the message to the buffer
00319     double min_value = buffer_[0];
00320     double max_value = buffer_[0];
00321     for (size_t i = 0; i < buffer_length_ - 1; i++) {
00322       buffer_[i] = buffer_[i + 1];
00323       if (min_value > buffer_[i]) {
00324         min_value = buffer_[i];
00325       }
00326       if (max_value < buffer_[i]) {
00327         max_value = buffer_[i];
00328       }
00329     }
00330     buffer_[buffer_length_ - 1] = msg->data;
00331     if (min_value > msg->data) {
00332       min_value = msg->data;
00333     }
00334     if (max_value < msg->data) {
00335       max_value = msg->data;
00336     }
00337     if (auto_scale_) {
00338       min_value_ = min_value;
00339       max_value_ = max_value;
00340       if (min_value_ == max_value_) {
00341         min_value_ = min_value_ - 0.5;
00342         max_value_ = max_value_ + 0.5;
00343       }
00344     }
00345     if (!overlay_->isVisible()) {
00346       return;
00347     }
00348     
00349     draw_required_ = true;
00350   }
00351 
00352   void Plotter2DDisplay::update(float wall_dt, float ros_dt)
00353   {
00354     if (draw_required_) {
00355       if (wall_dt + last_time_ > update_interval_) {
00356         overlay_->updateTextureSize(texture_width_,
00357                                     texture_height_ + caption_offset_);
00358         overlay_->setPosition(left_, top_);
00359         overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
00360         last_time_ = 0;
00361         drawPlot();
00362         draw_required_ = false;
00363       }
00364       else {
00365         last_time_ = last_time_ + wall_dt;
00366       }
00367     }
00368   }
00369   
00370   void Plotter2DDisplay::subscribe()
00371   {
00372     initializeBuffer();
00373     std::string topic_name = update_topic_property_->getTopicStd();
00374     if (topic_name.length() > 0 && topic_name != "/") {
00375       ros::NodeHandle n;
00376       sub_ = n.subscribe(topic_name, 1, &Plotter2DDisplay::processMessage, this);
00377     }
00378   }
00379 
00380   void Plotter2DDisplay::unsubscribe()
00381   {
00382     sub_.shutdown();
00383   }
00384 
00385   void Plotter2DDisplay::onEnable()
00386   {
00387     last_time_ = 0;
00388     draw_required_ = false;
00389     subscribe();
00390     overlay_->show();
00391   }
00392 
00393   void Plotter2DDisplay::onDisable()
00394   {
00395     unsubscribe();
00396     overlay_->hide();
00397   }
00398 
00399   void Plotter2DDisplay::updateWidth()
00400   {
00401     boost::mutex::scoped_lock lock(mutex_);
00402     texture_width_ = width_property_->getInt();
00403   }
00404   
00405   void Plotter2DDisplay::updateHeight()
00406   {
00407     boost::mutex::scoped_lock lock(mutex_);
00408     texture_height_ = height_property_->getInt();
00409   }
00410   
00411   void Plotter2DDisplay::updateTop()
00412   {
00413     top_ = top_property_->getInt();
00414   }
00415   
00416   void Plotter2DDisplay::updateLeft()
00417   {
00418     left_ = left_property_->getInt();
00419   }
00420   
00421   void Plotter2DDisplay::updateBGColor()
00422   {
00423     bg_color_ = bg_color_property_->getColor();
00424   }
00425 
00426   void Plotter2DDisplay::updateFGColor()
00427   {
00428     fg_color_ = fg_color_property_->getColor();
00429   }
00430 
00431   void Plotter2DDisplay::updateFGAlpha()
00432   {
00433     fg_alpha_ = fg_alpha_property_->getFloat() * 255.0;
00434   }
00435 
00436   void Plotter2DDisplay::updateBGAlpha()
00437   {
00438     bg_alpha_ = bg_alpha_property_->getFloat() * 255.0;
00439   }
00440   
00441   void Plotter2DDisplay::updateTopic()
00442   {
00443     unsubscribe();
00444     subscribe();
00445   }
00446 
00447   void Plotter2DDisplay::updateShowValue()
00448   {
00449     show_value_ = show_value_property_->getBool();
00450   }
00451   
00452   void Plotter2DDisplay::updateShowBorder()
00453   {
00454     show_border_ = show_border_property_->getBool();
00455   }
00456   
00457   void Plotter2DDisplay::updateLineWidth()
00458   {
00459     line_width_ = line_width_property_->getInt();
00460   }
00461   
00462   void Plotter2DDisplay::updateBufferSize()
00463   {
00464     buffer_length_ = buffer_length_property_->getInt();
00465     initializeBuffer();
00466   }
00467 
00468   void Plotter2DDisplay::updateAutoColorChange()
00469   {
00470     auto_color_change_ = auto_color_change_property_->getBool();
00471     if (auto_color_change_) {
00472       max_color_property_->show();
00473     }
00474     else {
00475       max_color_property_->hide();
00476     }
00477   }
00478 
00479   void Plotter2DDisplay::updateMaxColor()
00480   {
00481     max_color_ = max_color_property_->getColor();
00482   }
00483   
00484   void Plotter2DDisplay::updateUpdateInterval()
00485   {
00486     update_interval_ = update_interval_property_->getFloat();
00487   }
00488 
00489   void Plotter2DDisplay::updateTextSize()
00490   {
00491     text_size_ = text_size_property_->getInt();
00492     QFont font;
00493     font.setPointSize(text_size_);
00494     caption_offset_ = QFontMetrics(font).height();
00495   }
00496   
00497   void Plotter2DDisplay::updateShowCaption()
00498   {
00499     show_caption_  = show_caption_property_->getBool();
00500     if (show_caption_) {
00501       text_size_property_->show();
00502     }
00503     else {
00504       text_size_property_->hide();
00505     }
00506   }
00507 
00508   void Plotter2DDisplay::updateMinValue()
00509   {
00510     if (!auto_scale_) {
00511       min_value_ = min_value_property_->getFloat();
00512     }
00513   }
00514 
00515   void Plotter2DDisplay::updateMaxValue()
00516   {
00517     if (!auto_scale_) {
00518       max_value_ = max_value_property_->getFloat();
00519     }
00520   }
00521 
00522   void Plotter2DDisplay::updateAutoScale()
00523   {
00524     auto_scale_ = auto_scale_property_->getBool();
00525     if (auto_scale_) {
00526       min_value_property_->hide();
00527       max_value_property_->hide();
00528     }
00529     else {
00530       min_value_property_->show();
00531       max_value_property_->show();
00532     }
00533     updateMinValue();
00534     updateMaxValue();
00535   }
00536 
00537 }
00538 
00539 #include <pluginlib/class_list_macros.h>
00540 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::Plotter2DDisplay, rviz::Display )


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