pictogram_display.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014, JSK Lab
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include "pictogram_display.h"
00036 #include <QPainter>
00037 #include <QFontDatabase>
00038 #include <ros/package.h>
00039 
00041 // read Entypo fonts
00042 // http://mempko.wordpress.com/2014/11/28/using-entypo-fonts-as-icons-in-your-qt-application/
00044 
00045 #include "Entypo.dat"
00046 #include "Entypo_Social.dat"
00047 #include "fontawesome.dat"
00048 #include "pictogram_font_mapping.h"
00049 
00050 namespace jsk_rviz_plugins
00051 {
00052 
00053   int addFont(unsigned char* data, unsigned int data_len)
00054   {
00055     // register font
00056     QByteArray entypo =
00057       QByteArray::fromRawData(
00058         reinterpret_cast<const char*>(data), data_len);
00059     int id =
00060       QFontDatabase::addApplicationFontFromData(entypo);
00061     if (id == -1) {
00062       ROS_WARN("failed to load font");
00063     }
00064     else {
00065       return id;
00066     }
00067   }  
00068   
00069   bool epsEqual(double a, double b)
00070   {
00071     return (std::abs(a - b) < 0.01);
00072   }
00073   
00074   bool isCharacterSupported(std::string character)
00075   {
00076     return ((entypo_social_character_map.find(character)
00077              != entypo_social_character_map.end()) ||
00078             (entypo_character_map.find(character)
00079              != entypo_character_map.end()) ||
00080             (fontawesome_character_map.find(character)
00081              != fontawesome_character_map.end()));
00082   }
00083   
00084   QFont getFont(std::string character)
00085   {
00086     if (entypo_social_character_map.find(character)
00087         != entypo_social_character_map.end()) {
00088       return QFont("Entypo Social");
00089     }
00090     else if (entypo_character_map.find(character)
00091              != entypo_character_map.end()) {
00092       return QFont("Entypo");
00093     }
00094     else {
00095       return QFont("FontAwesome");
00096     }
00097   }
00098   
00099   QString lookupPictogramText(std::string character)
00100   {
00101     if (entypo_social_character_map.find(character)
00102         != entypo_social_character_map.end()) {
00103       return entypo_social_character_map[character];
00104     }
00105     else if (entypo_character_map.find(character)
00106              != entypo_character_map.end()){
00107       return entypo_character_map[character];
00108     }
00109     else {
00110       return fontawesome_character_map[character];
00111     }
00112   }
00113 
00114   bool isEntypo(std::string text) {
00115     return ((entypo_social_character_map.find(text)
00116              != entypo_social_character_map.end()) ||
00117             (entypo_character_map.find(text)
00118              != entypo_character_map.end()));
00119   }
00120 
00121   bool isFontAwesome(std::string text) {
00122     return (fontawesome_character_map.find(text)
00123             != fontawesome_character_map.end());
00124   }
00125 
00126   
00127   
00128   PictogramObject::PictogramObject(Ogre::SceneManager* manager,
00129                                    Ogre::SceneNode* parent,
00130                                    double size):
00131     FacingTexturedObject(manager, parent, size),
00132     need_to_update_(false),
00133     action_(jsk_rviz_plugins::Pictogram::ADD)
00134   {
00135     square_object_->setPolygonType(SquareObject::SQUARE);
00136     square_object_->rebuildPolygon();
00137     
00138     // for (std::map<std::string, QString>::iterator it = fontawesome_character_map.begin();
00139     //      it != fontawesome_character_map.end();
00140     //      ++it) {
00141     //   ROS_INFO("%s", it->first.c_str());
00142     // }
00143   }
00144 
00145   void PictogramObject::setEnable(bool enable)
00146   {
00147     if (enable && !enable_) {
00148       need_to_update_ = true;
00149     }
00150     FacingTexturedObject::setEnable(enable);
00151   }
00152 
00153   void PictogramObject::start()
00154   {
00155     time_ = ros::WallTime::now();
00156   }
00157 
00158   void PictogramObject::setSize(double size)
00159   {
00160     if (size_ != size) {
00161       need_to_update_ = true;
00162       FacingTexturedObject::setSize(size);
00163     }
00164   }
00165 
00166   void PictogramObject::setSpeed(double speed)
00167   {
00168     speed_ = speed;
00169   }
00170 
00171   void PictogramObject::setPose(const geometry_msgs::Pose& pose,
00172                                 const std::string& frame_id)
00173   {
00174     pose_ = pose;
00175     frame_id_ = frame_id;
00176   }
00177   
00178   void PictogramObject::setContext(rviz::DisplayContext* context)
00179   {
00180     context_ = context;
00181   }
00182 
00183   void PictogramObject::setMode(uint8_t mode)
00184   {
00185     mode_ = mode;
00186   }
00187 
00188   void PictogramObject::setTTL(double ttl)
00189   {
00190     ttl_ = ttl;
00191   }
00192 
00193   void PictogramObject::setAction(uint8_t type)
00194   {
00195     action_ = type;
00196     if (action_ == jsk_rviz_plugins::Pictogram::DELETE) {
00197       setEnable(false);
00198     }
00199     else{
00200       start();
00201     }
00202   }
00203 
00204   void PictogramObject::updatePose(float wall_dt)
00205   {
00206     Ogre::Vector3 position;
00207     Ogre::Quaternion quaternion;
00208     std_msgs::Header header;
00209     header.frame_id = frame_id_;
00210     if(!context_->getFrameManager()->transform(header,
00211                                                pose_,
00212                                                position,
00213                                                quaternion)) {
00214       ROS_ERROR( "Error transforming pose from frame '%s'",
00215                  frame_id_.c_str());
00216       return;
00217     }
00218 
00219     if (action_ == jsk_rviz_plugins::Pictogram::ADD) {
00220       setPosition(position);
00221       setOrientation(quaternion);
00222     }
00223     else if (action_ == jsk_rviz_plugins::Pictogram::ROTATE_Z ||
00224              action_ == jsk_rviz_plugins::Pictogram::ROTATE_X ||
00225              action_ == jsk_rviz_plugins::Pictogram::ROTATE_Y) {
00226       Ogre::Vector3 axis;
00227       if (action_ == jsk_rviz_plugins::Pictogram::ROTATE_Z) {
00228         axis = Ogre::Vector3(0, 0, 1);
00229       }
00230       else if (action_ == jsk_rviz_plugins::Pictogram::ROTATE_X) {
00231         axis = Ogre::Vector3(1, 0, 0);
00232       }
00233       else if (action_ == jsk_rviz_plugins::Pictogram::ROTATE_Y) {
00234         axis = Ogre::Vector3(0, 1, 0);
00235       }
00236       time_ = time_ + ros::WallDuration(wall_dt);
00237       // time_ -> theta
00238       Ogre::Radian theta(M_PI * 2 * fmod(time_.toSec() * speed_, 1.0));
00239       
00240       Ogre::Quaternion offset;
00241       offset.FromAngleAxis(theta, axis);
00242       Ogre::Quaternion final_rot = quaternion * offset;
00243       setPosition(position);
00244       setOrientation(final_rot);
00245     }
00246     else if (action_ == jsk_rviz_plugins::Pictogram::JUMP ||
00247              action_ == jsk_rviz_plugins::Pictogram::JUMP_ONCE) {
00248       bool jumpingp = false;
00249       if (action_ == jsk_rviz_plugins::Pictogram::JUMP) {
00250         jumpingp = true;
00251       }
00252       else if (action_ == jsk_rviz_plugins::Pictogram::JUMP_ONCE &&
00253                (ros::WallTime::now() - time_).toSec() < 2) {
00254         jumpingp = true;
00255       }
00256       
00257       if (!jumpingp) {
00258         setPosition(position);
00259       }
00260       else {
00261         // t(2-t) * size
00262         double t = fmod((ros::WallTime::now() - time_).toSec(), 2.0);
00263         double height = size_ * t * (2 - t);
00264         Ogre::Vector3 new_pos = position + quaternion * Ogre::Vector3(height, 0, 0);
00265         setPosition(new_pos);
00266       }
00267       setOrientation(quaternion);
00268     }
00269 
00270     double exceeded_time;
00271     if( ttl_ && (exceeded_time = (ros::WallTime::now() - time_).toSec()) > ttl_) {
00272       setAlpha( std::max(1.0 - 1.0 * (ros::WallTime::now() - (time_ + ros::WallDuration(ttl_))).toSec() / 5.0, 0.0) );
00273       if( 1.0 - 1.0 * (ros::WallTime::now() - (time_ + ros::WallDuration(ttl_))).toSec() / 3.0 < 0)
00274         setAction(jsk_rviz_plugins::Pictogram::DELETE);
00275     }
00276   }
00277   
00278   void PictogramObject::update(float wall_dt, float ros_dt)
00279   {
00280     if (text_.empty()) {
00281       // not yet setted
00282       return;
00283     }
00284     // update position and orientation
00285     if (!context_) {
00286       return;
00287     }
00288     updatePose(wall_dt);
00289     if (!need_to_update_) {
00290       return;
00291     }
00292     need_to_update_ = false;
00293     ScopedPixelBuffer buffer = texture_object_->getBuffer();
00294     QColor transparent(255, 255, 255, 0);
00295     QImage Hud = buffer.getQImage(128, 128, transparent); // should change according to size
00296     QPainter painter( &Hud );
00297     painter.setRenderHint(QPainter::Antialiasing, true);
00298     QColor foreground = rviz::ogreToQt(color_);
00299     painter.setPen(QPen(foreground, 5, Qt::SolidLine));
00300     
00301     if (isCharacterSupported(text_) && mode_ == jsk_rviz_plugins::Pictogram::PICTOGRAM_MODE) {
00302       QFont font = getFont(text_);
00303       QString pictogram_text = lookupPictogramText(text_);
00304       if (isEntypo(text_)) {
00305         font.setPointSize(100);
00306       }
00307       else if (isFontAwesome(text_)) {
00308         font.setPointSize(45);
00309       }
00310       painter.setFont(font);
00311       painter.drawText(0, 0, 128, 128,
00312                        Qt::AlignHCenter | Qt::AlignVCenter,
00313                        pictogram_text);
00314       painter.end();
00315     }else if( mode_ == jsk_rviz_plugins::Pictogram::STRING_MODE){
00316       QFont font("Arial");
00317       font.setPointSize(32);
00318       font.setBold(true);
00319       painter.setFont(font);
00320       painter.drawText(0, 0, 128, 128,
00321                        Qt::TextWordWrap | Qt::AlignHCenter | Qt::AlignVCenter,
00322                        text_.c_str());
00323       painter.end();
00324     }
00325     else {
00326       ROS_WARN("%s is not supported", text_.c_str());
00327     }
00328   }
00329 
00330   void PictogramObject::updateColor()
00331   {
00332   }
00333   
00334   void PictogramObject::updateText()
00335   {
00336   }
00337 
00338   void PictogramObject::setColor(QColor color)
00339   {
00340     if (!epsEqual(color_.r * 255.0, color.red()) ||
00341         !epsEqual(color_.g * 255.0, color.green()) ||
00342         !epsEqual(color_.b * 255.0, color.blue())) {
00343       FacingTexturedObject::setColor(color);
00344       need_to_update_ = true;
00345     }
00346   }
00347 
00348   void PictogramObject::setText(std::string text)
00349   {
00350     if (text_ != text) {
00351       FacingTexturedObject::setText(text);
00352       need_to_update_ = true;
00353     }
00354   }
00355   
00356   void PictogramObject::setAlpha(double alpha)
00357   {
00358     if (!epsEqual(color_.a, alpha)) {
00359       need_to_update_ = true;
00360       FacingTexturedObject::setAlpha(alpha);
00361     }
00362   }
00363   
00364   PictogramDisplay::PictogramDisplay()
00365   {
00366     setupFont();
00367   }
00368 
00369   PictogramDisplay::~PictogramDisplay()
00370   {
00371     
00372   }
00373   
00374   void PictogramDisplay::onInitialize()
00375   {
00376     MFDClass::onInitialize();
00377     pictogram_.reset(new PictogramObject(scene_manager_,
00378                                          scene_node_,
00379                                          1.0));
00380 
00381     pictogram_->setContext(context_);
00382     pictogram_->setEnable(false);
00383     pictogram_->start();
00384     // initial setting
00385     pictogram_->setColor(QColor(25, 255, 240));
00386     pictogram_->setAlpha(1.0);
00387     pictogram_->setSpeed(1.0);
00388     scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00389   }
00390 
00391   void PictogramDisplay::reset()
00392   {
00393     MFDClass::reset();
00394     pictogram_->setEnable(false);
00395   }
00396 
00397   void PictogramDisplay::onEnable()
00398   {
00399     subscribe();
00400     if (pictogram_) {
00401       // keep false, it will be true
00402       // in side of processMessae callback.
00403       pictogram_->setEnable(false);
00404     }
00405   }
00406 
00407   void PictogramDisplay::processMessage(const jsk_rviz_plugins::Pictogram::ConstPtr& msg)
00408   {
00409     boost::mutex::scoped_lock (mutex_);
00410 
00411     pictogram_->setEnable(isEnabled());
00412     if (!isEnabled()) {
00413       return;
00414     }
00415     pictogram_->setAction(msg->action);
00416     if (msg->action == jsk_rviz_plugins::Pictogram::DELETE) {
00417       return;
00418     }
00419     
00420     if (msg->size <= 0.0) {
00421       pictogram_->setSize(0.5);
00422     }
00423     else {
00424       pictogram_->setSize(msg->size / 2.0);
00425     }
00426     pictogram_->setColor(QColor(msg->color.r * 255,
00427                                 msg->color.g * 255,
00428                                 msg->color.b * 255));
00429     pictogram_->setAlpha(msg->color.a);
00430     pictogram_->setPose(msg->pose, msg->header.frame_id);
00431     pictogram_->setText(msg->character);
00432     pictogram_->setMode(msg->mode);
00433     pictogram_->setTTL(msg->ttl);
00434     if (msg->speed)
00435       pictogram_->setSpeed(msg->speed);
00436   }
00437 
00438   void PictogramDisplay::update(float wall_dt, float ros_dt)
00439   {
00440     boost::mutex::scoped_lock (mutex_);
00441     if (pictogram_) {
00442       pictogram_->update(wall_dt, ros_dt);
00443     }
00444   }
00445 }
00446 
00447 #include <pluginlib/class_list_macros.h>
00448 PLUGINLIB_EXPORT_CLASS (jsk_rviz_plugins::PictogramDisplay, rviz::Display);


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