Go to Qiita Advent Calendar 2024 Top
LoginSignup
3
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@srs(project_srs)

ROS講座118 overlayで表示するのrviz display plugindを作る

Last updated at Posted at 2020-07-12

環境

この記事は以下の環境で動いています。

項目 値
CPU Core i5-8250U
Ubuntu 20.04
ROS Kinetic
Qt 5.12.8

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

Rvizのメインの3D画面の3D空間の中ではなく、画面上の位置に情報を表示するプラグラインがjsk_rviz_pluginなどであります。
これは3D描画のエンジンであるOgreのOverviewの機能を使って描画されています。
今回はこの機能を使ってみます。RvizPluginの種類としては前回使用したPoint型を表示したものと同じDisplayタイプのpluginsになります。これを使って、/rosoutに表示されるログを表示するpluginを作ります。

ソースコード

plugin_lecture/src/rviz/overlay/log_display.cpp
#include "log_display.h"
#include <pluginlib/class_list_macros.h>
#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreTextureManager.h>
#include <OGRE/OgreTexture.h>
#include <OGRE/OgreTechnique.h>
#include <OGRE/OgreHardwarePixelBuffer.h>
#include <OGRE/Overlay/OgrePanelOverlayElement.h>
#include <OGRE/Overlay/OgreOverlayElement.h>
#include <OGRE/Overlay/OgreOverlayContainer.h>
#include <OGRE/Overlay/OgreOverlayManager.h>
#include <QImage>
#include <QColor>

namespace plugin_lecture
{
LogDisplay::LogDisplay()
{
  height_property_ = new rviz::IntProperty("height", 200, "window height", this, SLOT(renderOverlay()));
  width_property_ = new rviz::IntProperty("width", 400, "window width", this, SLOT(renderOverlay()));
  size_property_ = new rviz::IntProperty("size", 16, "window size", this, SLOT(renderOverlay()));

  overlay_ = Ogre::OverlayManager::getSingleton().create("RvizLogOverlay");
  // overlay text
  panel_ = (Ogre::OverlayContainer*)Ogre::OverlayManager::getSingleton().createOverlayElement("Panel", "TextPanel");
  overlay_->add2D(panel_);

  // overlay material
  createMaterial("BackGround");
  mat_element_ = Ogre::OverlayManager::getSingleton().createOverlayElement("Panel", "TexturePanel");
  mat_element_->setMetricsMode(Ogre::GuiMetricsMode::GMM_PIXELS);
  mat_element_->setMaterialName("BackGround");
  overlay_->add2D((Ogre::OverlayContainer*)mat_element_);

  overlay_->show();

  log_sub_ = nh_.subscribe("/rosout", 10, &LogDisplay::processMessage, this);
}

LogDisplay::~LogDisplay()
{
  Ogre::OverlayManager::getSingleton().destroy("RvizLogOverlay");
  // text
  Ogre::OverlayManager::getSingleton().destroyOverlayElement(panel_);
  for (auto element : text_elements_)
    Ogre::OverlayManager::getSingleton().destroyOverlayElement(element);
  // material
  Ogre::OverlayManager::getSingleton().destroyOverlayElement(mat_element_);
  destroyMaterial("BackGround");
}

void LogDisplay::save(rviz::Config config) const
{
  rviz::Display::save(config);
  config.mapSetValue("Height", height_property_->getInt());
  config.mapSetValue("Width", width_property_->getInt());
  config.mapSetValue("Size", size_property_->getInt());
}

void LogDisplay::load(const rviz::Config& config)
{
  rviz::Display::load(config);
  int temp_int;
  if (config.mapGetInt("Height", &temp_int))
    height_property_->setInt(temp_int);
  if (config.mapGetInt("Width", &temp_int))
    width_property_->setInt(temp_int);
  if (config.mapGetInt("Size", &temp_int))
    size_property_->setInt(temp_int);
}

Ogre::TextAreaOverlayElement* LogDisplay::createTextElement(int index)
{
  std::string name = "Text" + std::to_string(index);
  Ogre::TextAreaOverlayElement* text_ptr =
      (Ogre::TextAreaOverlayElement*)Ogre::OverlayManager::getSingleton().createOverlayElement("TextArea", name);
  text_ptr->setMetricsMode(Ogre::GuiMetricsMode::GMM_PIXELS);
  text_ptr->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_TOP);
  text_ptr->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_LEFT);
  text_ptr->setColour(Ogre::ColourValue::White);
  text_ptr->setFontName("Liberation Sans");
  text_ptr->setCaption("hello world");
  text_ptr->setCharHeight(16);
  text_ptr->setPosition(16, 16 * (index + 1));
  return text_ptr;
}

void LogDisplay::processMessage(const rosgraph_msgs::Log& msg)
{
  if (msg.level == rosgraph_msgs::Log::DEBUG)
    return;

  log_msgs_.push_back(msg);
  int max_lines = height_property_->getInt() / size_property_->getInt();
  while (max_lines < log_msgs_.size())
    log_msgs_.pop_front();

  renderOverlay();
}

void LogDisplay::createMaterial(std::string mat_name)
{
  int width = 100;
  int height = 100;
  std::string texture_name = mat_name + "Texture";
  QColor bg_color(0, 0, 0, 70.0);

  Ogre::TexturePtr texture = Ogre::TextureManager::getSingleton().createManual(
      texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, width, height, 0,
      Ogre::PF_A8R8G8B8, Ogre::TU_DEFAULT);

  Ogre::MaterialPtr mat = (Ogre::MaterialPtr)Ogre::MaterialManager::getSingleton().create(
      mat_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true);

  mat->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name);
  mat->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);

  Ogre::HardwarePixelBufferSharedPtr pixel_buffer = texture->getBuffer();
  pixel_buffer->lock(Ogre::HardwareBuffer::HBL_NORMAL);
  const Ogre::PixelBox& pixelBox = pixel_buffer->getCurrentLock();
  Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
  memset(pDest, 0, width * height);
  QImage Hud = QImage(pDest, width, height, QImage::Format_ARGB32);
  for (unsigned int i = 0; i < width; i++)
  {
    for (unsigned int j = 0; j < height; j++)
    {
      Hud.setPixel(i, j, bg_color.rgba());
    }
  }
  pixel_buffer->unlock();
}

void LogDisplay::destroyMaterial(std::string mat_name)
{
  std::string texture_name = mat_name + "Texture";
  Ogre::TextureManager::getSingleton().destroyResourcePool(texture_name);
  Ogre::MaterialManager::getSingleton().destroyResourcePool(mat_name);
}

void LogDisplay::renderOverlay()
{
  if (!isEnabled())
  {
    overlay_->hide();
    return;
  }
  overlay_->show();

  int view_width = Ogre::OverlayManager::getSingleton().getViewportWidth();
  int window_width = width_property_->getInt();
  int window_height = height_property_->getInt();
  int text_size = size_property_->getInt();

  mat_element_->setDimensions(window_width, window_height);
  mat_element_->setPosition(view_width - window_width, 0);

  while (text_elements_.size() < log_msgs_.size())
  {
    int id = text_elements_.size();
    text_elements_.push_back(createTextElement(id));
    panel_->addChild(text_elements_.back());
  }
  for (int i = log_msgs_.size(); i < text_elements_.size(); i++)
  {
    text_elements_[i]->setCaption("");
  }

  for (int i = 0; i < log_msgs_.size(); i++)
  {
    if (log_msgs_[i].level == rosgraph_msgs::Log::FATAL || log_msgs_[i].level == rosgraph_msgs::Log::ERROR)
      text_elements_[i]->setColour(Ogre::ColourValue::Red);
    else if (log_msgs_[i].level == rosgraph_msgs::Log::WARN)
      text_elements_[i]->setColour(Ogre::ColourValue(1, 1, 0));
    else
      text_elements_[i]->setColour(Ogre::ColourValue::White);

    text_elements_[i]->setPosition(view_width - window_width, text_size * i);
    text_elements_[i]->setCharHeight(text_size);

    std::string text = "[" + log_msgs_[i].name + "] " + log_msgs_[i].msg;
    text_elements_[i]->setCaption(text);
  }
}

}  // namespace plugin_lecture

PLUGINLIB_EXPORT_CLASS(plugin_lecture::LogDisplay, rviz::Display)
  • Ogre::OverlayManager::getSingleton().create()でoverlay実体を取得します。この下にテキスト用のelementと背景用のelementの2つを作っていきます。
  • Ogre::OverlayManager::getSingleton().createOverlayElement()でテキスト用のelementを生成します。この下にTextAreaOverlayElement型のelementを使うことで画面にテキストを表示します。
  • 背景を表示するのは少し複雑です。まず背景用のMaterialを生成します（Ogre::MaterialManager::getSingleton().create()）。これにTextureを生成して（Ogre::TextureManager::getSingleton().createManual()）セットします。Textureを希望する背景の色で塗りつぶします。

ビルド

cd ~/catkin_ws
catkin_make

実行

Rvizでpluginsを起動します。/rououtを出力するためにrosrun basic_lecture basic_loggerを起動します。
各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。

plugin_overlay.gif

参考

jsk_rviz_plugins

目次ページへのリンク

ROS講座の目次へのリンク

3
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
3
1

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?