void OverlayMenuDisplay::openingAnimation()
 {
   ROS_DEBUG("openningAnimation");
   prepareOverlay();
   int current_width = animation_t_ / animate_duration * overlay_->getTextureWidth();
   int current_height = animation_t_ / animate_duration * overlay_->getTextureHeight();
   {
     ScopedPixelBuffer buffer = overlay_->getBuffer();
     QColor bg_color(0, 0, 0, 255.0 / 2.0);
     QColor transparent(0, 0, 0, 0.0);
     QImage Hud = buffer.getQImage(*overlay_);
     for (int i = 0; i < overlay_->getTextureWidth(); i++) {
       for (int j = 0; j < overlay_->getTextureHeight(); j++) {
         if (i > (overlay_->getTextureWidth() - current_width) / 2.0 &&
             i < overlay_->getTextureWidth() - (overlay_->getTextureWidth() - current_width) / 2.0 &&
             j > (overlay_->getTextureHeight() - current_height) / 2.0 &&
             j < overlay_->getTextureHeight() - (overlay_->getTextureHeight() - current_height) / 2.0) {
           Hud.setPixel(i, j, bg_color.rgba());
         }
         else {
           Hud.setPixel(i, j, transparent.rgba());
         }
       }
     }
   }
   overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
   int window_width = context_->getViewManager()->getRenderPanel()->width();
   int window_height = context_->getViewManager()->getRenderPanel()->height();
   double window_left = (window_width - (int)overlay_->getTextureWidth()) / 2.0;
   double window_top = (window_height - (int)overlay_->getTextureHeight()) / 2.0;
   overlay_->setPosition(window_left, window_top);
                         
   current_menu_ = next_menu_;
 }
 void OverlayImageDisplay::redraw()
 {
   cv_bridge::CvImagePtr cv_ptr;
   try
   {
     if (msg_->width == 0 || msg_->height == 0) {
       // image width/height and texture width/height should be same
       // but they are not when input image width/height is 0
       return;
     }
     else if (msg_->encoding == sensor_msgs::image_encodings::RGBA8 ||
         msg_->encoding == sensor_msgs::image_encodings::BGRA8) {
       cv_ptr = cv_bridge::toCvCopy(msg_, sensor_msgs::image_encodings::RGBA8);
       cv::Mat mat = cv_ptr->image;
       ScopedPixelBuffer buffer = overlay_->getBuffer();
       QImage Hud = buffer.getQImage(*overlay_);
       for (int i = 0; i < overlay_->getTextureWidth(); i++) {
         for (int j = 0; j < overlay_->getTextureHeight(); j++) {
           QColor color(mat.data[j * mat.step + i * mat.elemSize() + 0],
                        mat.data[j * mat.step + i * mat.elemSize() + 1],
                        mat.data[j * mat.step + i * mat.elemSize() + 2],
                        mat.data[j * mat.step + i * mat.elemSize() + 3]);
           Hud.setPixel(i, j, color.rgba());
         }
       }
     }
     else {
       cv_ptr = cv_bridge::toCvCopy(msg_, sensor_msgs::image_encodings::RGB8);
       cv::Mat mat = cv_ptr->image;
       ScopedPixelBuffer buffer = overlay_->getBuffer();
       QImage Hud = buffer.getQImage(*overlay_);
       for (int i = 0; i < overlay_->getTextureWidth(); i++) {
         for (int j = 0; j < overlay_->getTextureHeight(); j++) {
           QColor color(mat.data[j * mat.step + i * mat.elemSize() + 0],
                        mat.data[j * mat.step + i * mat.elemSize() + 1],
                        mat.data[j * mat.step + i * mat.elemSize() + 2],
                        alpha_ * 255.0);
           Hud.setPixel(i, j, color.rgba());
         }
       }
     }
   }
   catch (cv_bridge::Exception& e)
   {
     ROS_ERROR("cv_bridge exception: %s", e.what());
   }
 }
 void GISCircleVisualizer::update(float wall_dt, float ros_dt)
 {
   ros::WallTime now = ros::WallTime::now();
   std::string text = text_ + " ";
   {
     ScopedPixelBuffer buffer = texture_object_->getBuffer();
     QColor transparent(0, 0, 0, 0);
     QColor foreground = rviz::ogreToQt(color_);
     QColor white(255, 255, 255, color_.a * 255);
     QImage Hud = buffer.getQImage(128, 128, transparent);
     double line_width = 5;
     double inner_line_width = 10;
     double l = 128;
     //double cx = l / 2 - line_width / 4.0;
     double cx = l / 2;
     //double cy = l / 2 - line_width / 4.0;
     double cy = l / 2;
     double r = 48;
     double inner_r = 40;
     double mouse_r = 30;
     double mouse_cy_offset = 5;
     
     QPainter painter( &Hud );
     painter.setRenderHint(QPainter::Antialiasing, true);
     painter.setPen(QPen(foreground, line_width, Qt::SolidLine));
     painter.setBrush(white);
     painter.drawEllipse(line_width / 2.0, line_width / 2.0,
                         l - line_width, l - line_width);
     double offset_rate = fmod(now.toSec(), 10) / 10.0;
     double theta_offset = offset_rate * M_PI * 2.0;
     for (size_t ci = 0; ci < text.length(); ci++) {
       double theta = M_PI * 2.0 / text.length() * ci + theta_offset;
       painter.save();
       QFont font("DejaVu Sans Mono");
       font.setPointSize(8);
       font.setBold(false);
       painter.setFont(font);
       painter.translate(cx + r * cos(theta),
                         cy + r * sin(theta));
       painter.rotate(theta / M_PI * 180 + 90);
       painter.drawText(0, 0, text.substr(ci, 1).c_str());
       painter.restore();
     }
     painter.setPen(QPen(foreground, inner_line_width, Qt::SolidLine));
     painter.setBrush(transparent);
     painter.drawEllipse(cx - inner_r, cy - inner_r,
                         inner_r * 2, inner_r * 2);
     double mouse_c_x = cx;
     double mouse_c_y = cy - mouse_cy_offset;
     double start_angle = -25 * M_PI / 180;
     double end_angle = -125 * M_PI / 180;
     painter.setPen(QPen(foreground, line_width, Qt::SolidLine));
     painter.drawChord(mouse_c_x - mouse_r, mouse_c_y - mouse_r,
                       2.0 * mouse_r, 2.0 * mouse_r,
                       start_angle * 180 / M_PI * 16,
                       end_angle * 180 / M_PI * 16);
     painter.end();
   }
 }
 void OverlayMenuDisplay::redraw()
 {
   ROS_DEBUG("redraw");
   prepareOverlay();
   {
     ScopedPixelBuffer buffer = overlay_->getBuffer();
     QColor bg_color(0, 0, 0, 255.0 / 2.0);
     QColor fg_color(25, 255, 240, 255.0);
     QImage Hud = buffer.getQImage(*overlay_, bg_color);
     QPainter painter( &Hud );
     painter.setRenderHint(QPainter::Antialiasing, true);
     painter.setPen(QPen(fg_color, 1, Qt::SolidLine));
     painter.setFont(font());
     int line_height = fontMetrics().height();
     int w = drawAreaWidth(next_menu_);
     painter.drawText(menu_padding_x,  menu_padding_y,
                      w, line_height,
                      Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
                      next_menu_->title.c_str());
     for (size_t i = 0; i < next_menu_->menus.size(); i++) {
       std::string menu = getMenuString(next_menu_, i);
       painter.drawText(menu_padding_x, line_height * ( 1 + i ) + menu_padding_y + menu_last_padding_y,
                        w, line_height,
                        Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
                        menu.c_str());
     }
     if (next_menu_->current_index <= next_menu_->menus.size()) {
       // draw '>'
       painter.drawText(menu_padding_x - fontMetrics().width(">") * 2,
                        line_height * ( 1 + next_menu_->current_index ) + menu_padding_y + menu_last_padding_y,
                        w, line_height,
                        Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
                        ">");
     }
     // draw line
     int texture_width = overlay_->getTextureWidth();
     int texture_height = overlay_->getTextureHeight();
     painter.drawLine(menu_padding_x / 2, menu_last_padding_y / 2 + line_height,
                      menu_padding_x / 2, texture_height - menu_last_padding_y / 2);
     painter.drawLine(texture_width - menu_padding_x / 2, menu_last_padding_y / 2 + line_height,
                      texture_width - menu_padding_x / 2, texture_height - menu_last_padding_y / 2);
     painter.drawLine(menu_padding_x / 2, menu_last_padding_y / 2 + line_height,
                      texture_width - menu_padding_x / 2, menu_last_padding_y / 2 + line_height);
     painter.drawLine(menu_padding_x / 2, texture_height - menu_last_padding_y / 2,
                      texture_width - menu_padding_x / 2, texture_height - menu_last_padding_y / 2);
     
     painter.end();
     current_menu_ = next_menu_;
   }
   overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
   int window_width = context_->getViewManager()->getRenderPanel()->width();
   int window_height = context_->getViewManager()->getRenderPanel()->height();
   double window_left = (window_width - (int)overlay_->getTextureWidth()) / 2.0;
   double window_top = (window_height - (int)overlay_->getTextureHeight()) / 2.0;
   overlay_->setPosition(window_left, window_top);
 }
Exemplo n.º 5
0
void OverlayTextDisplay::update(float wall_dt, float ros_dt)
{
    if (!require_update_texture_) {
        return;
    }
    if (!isEnabled()) {
        return;
    }
    if (!overlay_) {
        return;
    }
    overlay_->updateTextureSize(texture_width_, texture_height_);
    {
        ScopedPixelBuffer buffer = overlay_->getBuffer();
        QImage Hud = buffer.getQImage(*overlay_, bg_color_);
        QPainter painter( &Hud );
        painter.setRenderHint(QPainter::Antialiasing, true);
        painter.setPen(QPen(fg_color_, line_width_ || 1, Qt::SolidLine));
        uint16_t w = overlay_->getTextureWidth();
        uint16_t h = overlay_->getTextureHeight();

        // font
        if (text_size_ != 0) {
            //QFont font = painter.font();
            QFont font(font_.length() > 0 ? font_.c_str(): "Arial");
            font.setPointSize(text_size_);
            font.setBold(true);
            painter.setFont(font);
        }
        if (text_.length() > 0) {
            // painter.drawText(0, 0, w, h,
            //                  Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
            //                  text_.c_str());
            std::string color_wrapped_text
                = (boost::format("<span style=\"color: rgba(%2%, %3%, %4%, %5%)\">%1%</span>")
                   % text_ % fg_color_.red() % fg_color_.green() % fg_color_.blue() %
                   fg_color_.alpha()).str();
            QStaticText static_text(
                boost::algorithm::replace_all_copy(color_wrapped_text, "\n", "<br >").c_str());
            static_text.setTextWidth(w);
            painter.drawStaticText(0, 0, static_text);
        }
        painter.end();
    }
    overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
    require_update_texture_ = false;
}
 void PictogramObject::update(float wall_dt, float ros_dt)
 {
   if (text_.empty()) {
     // not yet setted
     return;
   }
   // update position and orientation
   if (!context_) {
     return;
   }
   updatePose(wall_dt);
   if (!need_to_update_) {
     return;
   }
   need_to_update_ = false;
   ScopedPixelBuffer buffer = texture_object_->getBuffer();
   QColor transparent(255, 255, 255, 0);
   QImage Hud = buffer.getQImage(128, 128, transparent); // should change according to size
   QPainter painter( &Hud );
   painter.setRenderHint(QPainter::Antialiasing, true);
   QColor foreground = rviz::ogreToQt(color_);
   painter.setPen(QPen(foreground, 5, Qt::SolidLine));
   
   if (isCharacterSupported(text_)) {
     QFont font = getFont(text_);
     QString pictogram_text = lookupPictogramText(text_);
     if (isEntypo(text_)) {
       font.setPointSize(100);
     }
     else if (isFontAwesome(text_)) {
       font.setPointSize(45);
     }
     painter.setFont(font);
     painter.drawText(0, 0, 128, 128,
                      Qt::AlignHCenter | Qt::AlignVCenter,
                      pictogram_text);
     painter.end();
   }
   else {
     ROS_WARN("%s is not supported", text_.c_str());
   }
 }
  void OverlayTextDisplay::update(float wall_dt, float ros_dt)
  {
    if (!require_update_texture_) {
      return;
    }
    if (!isEnabled()) {
      return;
    }
    if (!overlay_) {
      return;
    }
    overlay_->updateTextureSize(texture_width_, texture_height_);
    {
      ScopedPixelBuffer buffer = overlay_->getBuffer();
      QImage Hud = buffer.getQImage(*overlay_, bg_color_);
      QPainter painter( &Hud );
      painter.setRenderHint(QPainter::Antialiasing, true);
      painter.setPen(QPen(fg_color_, line_width_ || 1, Qt::SolidLine));
      uint16_t w = overlay_->getTextureWidth();
      uint16_t h = overlay_->getTextureHeight();

      // font
      if (text_size_ != 0) {
        //QFont font = painter.font();
        QFont font(font_.length() > 0 ? font_.c_str(): "Arial");
        font.setPointSize(text_size_);
        font.setBold(true);
        painter.setFont(font);
      }
      if (text_.length() > 0) {
        //painter.drawText(0, 0, w, h, Qt::TextWordWrap | Qt::AlignLeft,
        painter.drawText(0, 0, w, h,
                         Qt::TextWordWrap | Qt::AlignLeft | Qt::AlignTop,
                         text_.c_str());
      }
      painter.end();
    }
    overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
    require_update_texture_ = false;
  }
  void PieChartDisplay::drawPlot(double val)
  {
    QColor fg_color(fg_color_);

    if (auto_color_change_) {
      double r
        = std::min(1.0, fabs((val - min_value_) / (max_value_ - min_value_)));
      fg_color.setRed((max_color_.red() - fg_color_.red()) * r
                      + fg_color_.red());
      fg_color.setGreen((max_color_.green() - fg_color_.green()) * r
                      + fg_color_.green());
      fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r
                       + fg_color_.blue());
    }

    
    QColor fg_color2(fg_color);
    QColor bg_color(bg_color_);
    QColor text_color(text_color_);
    fg_color.setAlpha(fg_alpha_);
    fg_color2.setAlpha(fg_alpha2_);
    bg_color.setAlpha(bg_alpha_);
    text_color.setAlpha(text_alpha_);
    int width = overlay_->getTextureWidth();
    int height = overlay_->getTextureHeight();
    {
      ScopedPixelBuffer buffer = overlay_->getBuffer();
      QImage Hud = buffer.getQImage(*overlay_, bg_color);
      QPainter painter( &Hud );
      painter.setRenderHint(QPainter::Antialiasing, true);

      const int outer_line_width = 5;
      const int value_line_width = 10;
      const int value_indicator_line_width = 2;
      const int value_padding = 5;

      const int value_aabb_offset
        = outer_line_width + value_padding + value_line_width / 2;
      
      painter.setPen(QPen(fg_color, outer_line_width, Qt::SolidLine));

      painter.drawEllipse(outer_line_width / 2, outer_line_width / 2,
                          width - outer_line_width ,
                          height - outer_line_width - caption_offset_);

      painter.setPen(QPen(fg_color2, value_indicator_line_width, Qt::SolidLine));
      painter.drawEllipse(value_aabb_offset, value_aabb_offset,
                          width - value_aabb_offset * 2,
                          height - value_aabb_offset * 2 - caption_offset_);

      const double ratio = (val - min_value_) / (max_value_ - min_value_);
      const double ratio_angle = ratio * 360.0;
      const double start_angle_offset = -90;
      painter.setPen(QPen(fg_color, value_line_width, Qt::SolidLine));
      painter.drawArc(QRectF(value_aabb_offset, value_aabb_offset,
                             width - value_aabb_offset * 2,
                             height - value_aabb_offset * 2 - caption_offset_),
                      start_angle_offset * 16 ,
                      ratio_angle * 16);
      QFont font = painter.font();
      font.setPointSize(text_size_);
      font.setBold(true);
      painter.setFont(font);
      painter.setPen(QPen(text_color, value_line_width, Qt::SolidLine));
      std::ostringstream s;
      s << std::fixed << std::setprecision(2) << val;
      painter.drawText(0, 0, width, height - caption_offset_,
                       Qt::AlignCenter | Qt::AlignVCenter,
                       s.str().c_str());

      // caption
      if (show_caption_) {
        painter.drawText(0, height - caption_offset_, width, caption_offset_,
                         Qt::AlignCenter | Qt::AlignVCenter,
                         getName());
      }
      
      // done
      painter.end();
      // Unlock the pixel buffer
    }
  }
  void Plotter2DDisplay::drawPlot()
  {
    QColor fg_color(fg_color_);
    QColor bg_color(bg_color_);
    
    fg_color.setAlpha(fg_alpha_);
    bg_color.setAlpha(bg_alpha_);

    if (auto_color_change_) {
      double r
        = std::min(std::max((buffer_[buffer_.size() - 1] - min_value_) / (max_value_ - min_value_),
                            0.0), 1.0);
      if (r > 0.3) {
        double r2 = (r - 0.3) / 0.7;
        fg_color.setRed((max_color_.red() - fg_color_.red()) * r2
                        + fg_color_.red());
        fg_color.setGreen((max_color_.green() - fg_color_.green()) * r2
                          + fg_color_.green());
        fg_color.setBlue((max_color_.blue() - fg_color_.blue()) * r2
                         + fg_color_.blue());
      }
    }
    
    {
      ScopedPixelBuffer buffer = overlay_->getBuffer();
      QImage Hud = buffer.getQImage(*overlay_);
      // initilize by the background color
      for (int i = 0; i < overlay_->getTextureWidth(); i++) {
        for (int j = 0; j < overlay_->getTextureHeight(); j++) {
          Hud.setPixel(i, j, bg_color.rgba());
        }
      }
      // paste in HUD speedometer. I resize the image and offset it by 8 pixels from
      // the bottom left edge of the render window
      QPainter painter( &Hud );
      painter.setRenderHint(QPainter::Antialiasing, true);
      painter.setPen(QPen(fg_color, line_width_, Qt::SolidLine));
      
      uint16_t w = overlay_->getTextureWidth();
      uint16_t h = overlay_->getTextureHeight() - caption_offset_;

      double margined_max_value = max_value_ + (max_value_ - min_value_) / 2;
      double margined_min_value = min_value_ - (max_value_ - min_value_) / 2;
      
      for (size_t i = 1; i < buffer_length_; i++) {
        double v_prev = (margined_max_value - buffer_[i - 1]) / (margined_max_value - margined_min_value);
        double v = (margined_max_value - buffer_[i]) / (margined_max_value - margined_min_value);
        double u_prev = (i - 1) / (float)buffer_length_;
        double u = i / (float)buffer_length_;

        // chop within 0 ~ 1
        v_prev = std::max(std::min(v_prev, 1.0), 0.0);
        u_prev = std::max(std::min(u_prev, 1.0), 0.0);
        v = std::max(std::min(v, 1.0), 0.0);
        u = std::max(std::min(u, 1.0), 0.0);
        
        uint16_t x_prev = (int)(u_prev * w);
        uint16_t x = (int)(u * w);
        uint16_t y_prev = (int)(v_prev * h);
        uint16_t y = (int)(v * h);
        painter.drawLine(x_prev, y_prev, x, y);
      }
      // draw border
      if (show_border_) {
        painter.drawLine(0, 0, 0, h);
        painter.drawLine(0, h, w, h);
        painter.drawLine(w, h, w, 0);
        painter.drawLine(w, 0, 0, 0);
      }
      // draw caption
      if (show_caption_) {
        QFont font = painter.font();
        font.setPointSize(text_size_);
        font.setBold(true);
        painter.setFont(font);
        painter.drawText(0, h, w, caption_offset_,
                         Qt::AlignCenter | Qt::AlignVCenter,
                         getName());
      }
      if (show_value_) {
        QFont font = painter.font();
        font.setPointSize(w / 4);
        font.setBold(true);
        painter.setFont(font);
        std::ostringstream ss;
        ss << std::fixed << std::setprecision(2) << buffer_[buffer_.size() - 1];
        painter.drawText(0, 0, w, h,
                         Qt::AlignCenter | Qt::AlignVCenter,
                         ss.str().c_str());
      }
      
      // done
      painter.end();
    }
  }