overlay_menu_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_menu_display.h"
00037 
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 
00052   const int menu_padding_x = 100;
00053   const int menu_padding_y = 5;
00054   const int menu_last_padding_y = 30;
00055   const double animate_duration = 0.2;
00056   OverlayMenuDisplay::OverlayMenuDisplay() : Display()
00057   {
00058     update_topic_property_ = new rviz::RosTopicProperty(
00059       "Topic", "",
00060       ros::message_traits::datatype<jsk_rviz_plugins::OverlayMenu>(),
00061       "jsk_rviz_plugins::OverlayMenu topic to subscribe to.",
00062       this, SLOT( updateTopic() ));
00063  
00064   }
00065 
00066   OverlayMenuDisplay::~OverlayMenuDisplay()
00067   {
00068     onDisable();
00069     delete update_topic_property_;
00070   }
00071 
00072   void OverlayMenuDisplay::onInitialize()
00073   {
00074     require_update_texture_ = false;
00075     animation_state_ = CLOSED;
00076   }
00077   
00078   void OverlayMenuDisplay::onEnable()
00079   {
00080     if (overlay_) {
00081       overlay_->show();
00082     }
00083     subscribe();
00084   }
00085   void OverlayMenuDisplay::onDisable()
00086   {
00087     if (overlay_) {
00088       overlay_->hide();
00089     }
00090     unsubscribe();
00091   }
00092 
00093   void OverlayMenuDisplay::unsubscribe()
00094   {
00095     sub_.shutdown();
00096   }
00097 
00098   void OverlayMenuDisplay::subscribe()
00099   {
00100     std::string topic_name = update_topic_property_->getTopicStd();
00101     if (topic_name.length() > 0 && topic_name != "/") {
00102       sub_ = ros::NodeHandle().subscribe(topic_name, 1,
00103                                          &OverlayMenuDisplay::processMessage,
00104                                          this);
00105     }
00106   }
00107 
00108   void OverlayMenuDisplay::processMessage
00109   (const jsk_rviz_plugins::OverlayMenu::ConstPtr& msg)
00110   {
00111     next_menu_ = msg;
00112   }
00113 
00114   bool OverlayMenuDisplay::isNeedToResize()
00115   {
00116     if (!current_menu_ && next_menu_) { // first time
00117       ROS_DEBUG("need to resize because this is the first time to draw");
00118       return true;
00119     }
00120     else if (!current_menu_ && !next_menu_) {
00121       // both are null, it means that ...
00122       // the plugin tries to draw without message reception
00123       ROS_DEBUG("no need to resize because the plugin tries to draw without message reception");
00124       return false;
00125     }
00126     else if (current_menu_ && !next_menu_) {
00127       // this is unexpected case
00128       ROS_DEBUG("no need to resize, this is unexpected case. please debug");
00129       return false;
00130     }
00131     else {
00132       if (current_menu_->menus.size() != next_menu_->menus.size()) {
00133         ROS_DEBUG("need to resize because the length of menu is different");
00134         return true;
00135       }
00136       else if (current_menu_->title != next_menu_->title) {
00137         return true;
00138       }
00139       else {
00140         // check all the menu is same or not
00141         for (size_t i = 0; i < current_menu_->menus.size(); i++) {
00142           if (current_menu_->menus[i] != next_menu_->menus[i]) {
00143             ROS_DEBUG("need to resize because the content of menu is different");
00144             return true;
00145           }
00146         }
00147         ROS_DEBUG("no need to resize because the content of menu is same");
00148         return false;
00149       }
00150     }
00151   }
00152 
00153   QFont OverlayMenuDisplay::font()
00154   {
00155     QFont font;
00156     font.setPointSize(20);
00157     return font;
00158   }
00159   
00160   QFontMetrics OverlayMenuDisplay::fontMetrics()
00161   {
00162     QFontMetrics fm(font());
00163     return fm;
00164   }
00165   
00166   int OverlayMenuDisplay::drawAreaWidth(
00167     const jsk_rviz_plugins::OverlayMenu::ConstPtr& msg)
00168   {
00169     QFontMetrics fm = fontMetrics();
00170     int max_width = 0;
00171     for (size_t i = 0; i < msg->menus.size(); i++) {
00172       int w = fm.width(getMenuString(msg, i).c_str());
00173       if (max_width < w) {
00174         max_width = w;
00175       }
00176     }
00177     int w = fm.width(msg->title.c_str());
00178     
00179     if (max_width < w) {
00180       max_width = w;
00181     }
00182     return max_width + menu_padding_x * 2;
00183   }
00184 
00185   int OverlayMenuDisplay::drawAreaHeight(
00186     const jsk_rviz_plugins::OverlayMenu::ConstPtr& msg)
00187   {
00188     QFontMetrics fm = fontMetrics();
00189     return fm.height() * (msg->menus.size() + 1)
00190       + menu_padding_y * (msg->menus.size() + 1 - 1)
00191       + menu_last_padding_y * 2;
00192   }
00193   
00194   void OverlayMenuDisplay::update(float wall_dt, float ros_dt)
00195   {
00196     if (!next_menu_) {
00197       ROS_DEBUG("next_menu_ is null, no need to update");
00198       return;
00199     }
00200     if (next_menu_->action == jsk_rviz_plugins::OverlayMenu::ACTION_CLOSE &&
00201         animation_state_ == CLOSED) {
00202       ROS_DEBUG("request is close and state is closed, we ignore it completely");
00203       return;
00204     }
00205 
00206     if (next_menu_->action == jsk_rviz_plugins::OverlayMenu::ACTION_CLOSE) {
00207       // need to close...
00208       if (animation_state_ == CLOSED) {
00209         // do nothing, it should be ignored above if sentence
00210         ROS_WARN("request is CLOSE and state is CLOSED, it should be ignored before...");
00211       }
00212       else if (animation_state_ == OPENED) { // OPENED -> CLOSING
00213         animation_state_ = CLOSING;
00214         animation_t_ = animate_duration;
00215       }
00216       else if (animation_state_ == CLOSING) {
00217         animation_t_ -= wall_dt;
00218         if (animation_t_ > 0) { // CLOSING -> CLOSING
00219           openingAnimation();
00220         }
00221         else { // CLOSING -> CLOSED
00222           animation_t_ = 0;
00223           openingAnimation();
00224           animation_state_ = CLOSED;
00225         }
00226       }
00227       else if (animation_state_ == OPENING) { // if the status is OPENING, we open it anyway...??
00228         animation_t_ += wall_dt;
00229         if (animation_t_ < animate_duration) { // OPENING -> OPENING
00230           openingAnimation();
00231         }
00232         else {                  // OPENING -> OPENED
00233           redraw();
00234           animation_state_ = OPENED;
00235         }
00236       }
00237     }
00238     else {                      // OPEN request
00239       if (animation_state_ == CLOSED) { // CLOSED -> OPENING, do nothing just change the state
00240         animation_t_ = 0.0;
00241         animation_state_ = OPENING;
00242       }
00243       else if (animation_state_ == OPENING) {
00244         animation_t_ += wall_dt;
00245         ROS_DEBUG("animation_t: %f", animation_t_);
00246         if (animation_t_ < animate_duration) { // OPENING -> OPENING
00247           openingAnimation();
00248         }
00249         else {                  // OPENING -> OPENED
00250           redraw();
00251           animation_state_ = OPENED;
00252         }
00253       }
00254       else if (animation_state_ == OPENED) { // OPENED -> OPENED
00255         if (isNeedToRedraw()) {
00256           redraw();
00257         }
00258       }
00259       else if (animation_state_ == CLOSING) { // CLOSING, we close it anyway...
00260         animation_t_ -= wall_dt;
00261         if (animation_t_ > 0) {
00262           openingAnimation();
00263         }
00264         else {
00265           animation_t_ = 0;
00266           openingAnimation();
00267           animation_state_ = CLOSED;
00268         }
00269       }
00270     }
00271     //redraw();
00272     //current_menu_ = next_menu_;
00273   }
00274 
00275   bool OverlayMenuDisplay::isNeedToRedraw() {
00276     return true;
00277   }
00278 
00279   std::string OverlayMenuDisplay::getMenuString(
00280     const jsk_rviz_plugins::OverlayMenu::ConstPtr& msg,
00281     size_t index)
00282   {
00283     if (index >= msg->menus.size()) {
00284       return "";
00285     }
00286     else {
00287       return msg->menus[index];
00288     }
00289   }
00290 
00291   void OverlayMenuDisplay::prepareOverlay()
00292   {
00293     if (!overlay_) {
00294       static int count = 0;
00295       rviz::UniformStringStream ss;
00296       ss << "OverlayMenuDisplayObject" << count++;
00297       overlay_.reset(new OverlayObject(ss.str()));
00298       overlay_->show();
00299     }
00300     if (!overlay_->isTextureReady() || isNeedToResize()) {
00301       overlay_->updateTextureSize(drawAreaWidth(next_menu_), drawAreaHeight(next_menu_));
00302     }
00303     else {
00304       ROS_DEBUG("no need to update texture size");
00305     }
00306   }
00307   
00308   void OverlayMenuDisplay::openingAnimation()
00309   {
00310     ROS_DEBUG("openningAnimation");
00311     prepareOverlay();
00312     int current_width = animation_t_ / animate_duration * overlay_->getTextureWidth();
00313     int current_height = animation_t_ / animate_duration * overlay_->getTextureHeight();
00314     {
00315       ScopedPixelBuffer buffer = overlay_->getBuffer();
00316       QColor bg_color(0, 0, 0, 255.0 / 2.0);
00317       QColor transparent(0, 0, 0, 0.0);
00318       QImage Hud = buffer.getQImage(*overlay_);
00319       for (int i = 0; i < overlay_->getTextureWidth(); i++) {
00320         for (int j = 0; j < overlay_->getTextureHeight(); j++) {
00321           if (i > (overlay_->getTextureWidth() - current_width) / 2.0 &&
00322               i < overlay_->getTextureWidth() - (overlay_->getTextureWidth() - current_width) / 2.0 &&
00323               j > (overlay_->getTextureHeight() - current_height) / 2.0 &&
00324               j < overlay_->getTextureHeight() - (overlay_->getTextureHeight() - current_height) / 2.0) {
00325             Hud.setPixel(i, j, bg_color.rgba());
00326           }
00327           else {
00328             Hud.setPixel(i, j, transparent.rgba());
00329           }
00330         }
00331       }
00332     }
00333     overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
00334     int window_width = context_->getViewManager()->getRenderPanel()->width();
00335     int window_height = context_->getViewManager()->getRenderPanel()->height();
00336     double window_left = (window_width - (int)overlay_->getTextureWidth()) / 2.0;
00337     double window_top = (window_height - (int)overlay_->getTextureHeight()) / 2.0;
00338     overlay_->setPosition(window_left, window_top);
00339                           
00340     current_menu_ = next_menu_;
00341   }
00342   
00343   void OverlayMenuDisplay::redraw()
00344   {
00345     ROS_DEBUG("redraw");
00346     prepareOverlay();
00347     {
00348       ScopedPixelBuffer buffer = overlay_->getBuffer();
00349       QColor bg_color(0, 0, 0, 255.0 / 2.0);
00350       QColor fg_color(25, 255, 240, 255.0);
00351       QImage Hud = buffer.getQImage(*overlay_, bg_color);
00352       QPainter painter( &Hud );
00353       painter.setRenderHint(QPainter::Antialiasing, true);
00354       painter.setPen(QPen(fg_color, 1, Qt::SolidLine));
00355       painter.setFont(font());
00356       int line_height = fontMetrics().height();
00357       int w = drawAreaWidth(next_menu_);
00358       painter.drawText(menu_padding_x,  menu_padding_y,
00359                        w, line_height,
00360                        Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
00361                        next_menu_->title.c_str());
00362       for (size_t i = 0; i < next_menu_->menus.size(); i++) {
00363         std::string menu = getMenuString(next_menu_, i);
00364         painter.drawText(menu_padding_x, line_height * ( 1 + i ) + menu_padding_y + menu_last_padding_y,
00365                          w, line_height,
00366                          Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
00367                          menu.c_str());
00368       }
00369       if (next_menu_->current_index <= next_menu_->menus.size()) {
00370         // draw '>'
00371         painter.drawText(menu_padding_x - fontMetrics().width(">") * 2,
00372                          line_height * ( 1 + next_menu_->current_index ) + menu_padding_y + menu_last_padding_y,
00373                          w, line_height,
00374                          Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
00375                          ">");
00376       }
00377       // draw line
00378       int texture_width = overlay_->getTextureWidth();
00379       int texture_height = overlay_->getTextureHeight();
00380       painter.drawLine(menu_padding_x / 2, menu_last_padding_y / 2 + line_height,
00381                        menu_padding_x / 2, texture_height - menu_last_padding_y / 2);
00382       painter.drawLine(texture_width - menu_padding_x / 2, menu_last_padding_y / 2 + line_height,
00383                        texture_width - menu_padding_x / 2, texture_height - menu_last_padding_y / 2);
00384       painter.drawLine(menu_padding_x / 2, menu_last_padding_y / 2 + line_height,
00385                        texture_width - menu_padding_x / 2, menu_last_padding_y / 2 + line_height);
00386       painter.drawLine(menu_padding_x / 2, texture_height - menu_last_padding_y / 2,
00387                        texture_width - menu_padding_x / 2, texture_height - menu_last_padding_y / 2);
00388       
00389       painter.end();
00390       current_menu_ = next_menu_;
00391     }
00392     overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
00393     int window_width = context_->getViewManager()->getRenderPanel()->width();
00394     int window_height = context_->getViewManager()->getRenderPanel()->height();
00395     double window_left = (window_width - (int)overlay_->getTextureWidth()) / 2.0;
00396     double window_top = (window_height - (int)overlay_->getTextureHeight()) / 2.0;
00397     overlay_->setPosition(window_left, window_top);
00398   }
00399   
00400   void OverlayMenuDisplay::updateTopic()
00401   {
00402     unsubscribe();
00403     subscribe();
00404   }
00405 }
00406 
00407 #include <pluginlib/class_list_macros.h>
00408 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugins::OverlayMenuDisplay, rviz::Display )


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