ROS講座101 ogreで3D描画をする

環境

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

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

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

概要

ここまでqtで文字の表示や簡単な画像を扱いましたが、ロボットなので3次元表示は欠かせません。Rvizの3D表示ではqtの画面にOGREという描画エンジンのウィンドウを入れて実現していきます。ここではその使い方を説明します。
個々の内容によってオリジナルの3Dビューのウィンドウを作れるようになりますが、rvizのdisplayイプラグインで真価を発揮します。

• 基本的な図形(座標原点、線、幅広い線、円柱、矢印)の表示
• メッシュの表示
• テキストなどのオーバーレイ

ソースコード

ノード本体:qt_lecture/src/render/qt_basic5.cpp
ライブラリ宣言：qt_lecture/src/render/qt_render.h

ライブラリ実装

qt_lecture/src/render/qt_render.cpp

#include "qt_render.h"

#include <OGRE/OgreMaterialManager.h>
#include <OGRE/OgreTextureManager.h>
#include <OGRE/OgreTexture.h>
#include <OGRE/OgreTechnique.h>
#include <OGRE/OgreEntity.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 <OGRE/Overlay/OgreTextAreaOverlayElement.h>
#include <QImage>
#include <QColor>
#include <QVBoxLayout>
#include <rviz/uniform_string_stream.h>
#include <rviz/visualization_manager.h>
#include <rviz/render_panel.h>
#include <rviz/mesh_loader.h>

MyRender::MyRender(QWidget* parent) : QDialog(parent)
{
  render_panel_ = new rviz::RenderPanel();
  QVBoxLayout* main_layout = new QVBoxLayout;
  main_layout->addWidget(render_panel_);
  setLayout(main_layout);

  render_panel_->show();

  manager_ = new rviz::VisualizationManager(render_panel_);
  render_panel_->initialize(manager_->getSceneManager(), manager_);
  manager_->initialize();
  manager_->startUpdate();

  scene_manager_ = manager_->getSceneManager();

  // visualize axis
  vis_axes_.reset(new rviz::Axes(scene_manager_));

  // visualize arrow
  vis_arrow_.reset(new rviz::Arrow(scene_manager_));
  Ogre::Vector3 arrow_pos(0, -1, 0);
  vis_arrow_->setPosition(arrow_pos);
  vis_arrow_->setColor(1, 0, 0, 1.0);
  Ogre::Vector3 arrow_dir(0, 0, 1.0);
  float arrow_length = arrow_dir.length();
  Ogre::Vector3 arrow_scale(arrow_length, arrow_length, arrow_length);
  vis_arrow_->setScale(arrow_scale);
  vis_arrow_->setDirection(arrow_dir);

  // visualize line
  vis_line_.reset(new rviz::Line(scene_manager_));
  Ogre::Vector3 start(1, -1, 0);
  Ogre::Vector3 end(1, -1, 1);
  vis_line_->setPoints(start, end);
  vis_line_->setColor(1, 1, 1, 1);

  // visualize billboard line
  vis_billboard_line_.reset(new rviz::BillboardLine(scene_manager_));
  vis_billboard_line_->setLineWidth(0.1);
  vis_billboard_line_->setColor(0, 1, 0, 1);
  Ogre::Vector3 p0(2, -1, 0);
  vis_billboard_line_->addPoint(p0);
  Ogre::Vector3 p1(2, 0, 0);
  vis_billboard_line_->addPoint(p1);
  Ogre::Vector3 p2(2, 0, 1);
  vis_billboard_line_->addPoint(p2);
  Ogre::Vector3 p3(2, 1, 1);
  vis_billboard_line_->addPoint(p3);

  // visualize cylinder
  vis_shape_.reset(new rviz::Shape(rviz::Shape::Cylinder, scene_manager_));
  Ogre::Vector3 shape_pos(0, 2, 0);
  vis_shape_->setPosition(shape_pos);
  Ogre::Quaternion shape_q(0.7, 0.7, 0, 0);  // (w, x, y, z)
  vis_shape_->setOrientation(shape_q);
  vis_shape_->setColor(0, 0, 1, 1);

  // visualize mesh
  vis_mesh_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
  std::string flag_resource = "package://qt_lecture/resources/board.dae";
  if (rviz::loadMeshFromResource(flag_resource).isNull())
  {
    printf("failed to load model resource '%s'.\n", flag_resource.c_str());
    return;
  }
  Ogre::Entity* entity = scene_manager_->createEntity(flag_resource);
  vis_mesh_->attachObject(entity);
  Ogre::Vector3 flag_pos(2, 2, 0);
  vis_mesh_->setPosition(flag_pos);

  // overlay material
  Ogre::Overlay* overlay = Ogre::OverlayManager::getSingleton().create("test03.Overlay");

  int width = 100;
  int height = 100;
  std::string matName = "testmaterial";
  std::string texture_name = "mytexture";
  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(
      matName, 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);
  QColor bg_color(0, 0, 0, 70.0);
  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();

  Ogre::OverlayElement* e = Ogre::OverlayManager::getSingleton().createOverlayElement("Panel", "TexturePanel");
  e->setMetricsMode(Ogre::GuiMetricsMode::GMM_PIXELS);
  e->setMaterialName(matName);
  e->setDimensions(152, 32);
  e->setPosition(30, 28);
  overlay->add2D((Ogre::OverlayContainer*)e);

  // overlay text
  Ogre::OverlayContainer* panel =
      (Ogre::OverlayContainer*)Ogre::OverlayManager::getSingleton().createOverlayElement("Panel", "TextPanel");
  Ogre::TextAreaOverlayElement* text =
      (Ogre::TextAreaOverlayElement*)Ogre::OverlayManager::getSingleton().createOverlayElement("TextArea", "test03."
                                                                                                           "TextArea");
  text->setMetricsMode(Ogre::GuiMetricsMode::GMM_PIXELS);
  text->setVerticalAlignment(Ogre::GuiVerticalAlignment::GVA_TOP);
  text->setHorizontalAlignment(Ogre::GuiHorizontalAlignment::GHA_LEFT);
  text->setColour(Ogre::ColourValue::White);
  text->setFontName("Liberation Sans");
  text->setCaption("hello world");
  text->setCharHeight(32);
  text->setPosition(32, 32);
  panel->addChild(text);
  overlay->add2D((Ogre::OverlayContainer*)panel);

  overlay->show();
}

• new rviz::RenderPanel()で画面を作り、それから new rviz::VisualizationManager(render_panel_)で作ったmanagerを使って各オブジェクトを描画していきます。例えばvis_arrow_.reset(new rviz::Arrow( scene_manager_));とすると矢印を表示するオブジェクトを作成することができます。矢印の形を変えたい場合は適宜この生成されたクラスに変更を加えていけば画面に反映されます。
• Ogreはグラフィック系の座標系のためかCylinderの描画では円柱の軸がy軸方向になります。またROSと違ってogreのクオータニオンは(w, x, y, z)の順なので注意が必要です。
• 表示できるものは以下のrviz/Objectのリファレンスを見れば表示できるクラスの一覧があるのですが、更新が古いみたいです。インストールされているROSのrvizのlibrary以下(/opt/ros/kinetic/include/rviz/ogre_helpers)を見るのが良いでしょう。
• mechの表示とオーバーレイはrvizのラッパーがなく直接ogreのapiをたたく必要があります。
• 特にオーバーレイの記述は複雑です。overlayオブジェクト(画面ごとに1つ)->Containerオブジェクト->textオブジェクト or materialオブジェクトの階層構造になっています。

ビルド

cd ~/catkin_ws
catkin build

実行

rvizのライブラリを使っているのでrosに依存します。
各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。

1つ目のターミナル

roscore

1つ目のターミナル

rosrun qt_lecture qt_basic5 

実行すると以下のような画面が表示されます。

参考

visualization_tutorialソースコード
rviz/Objectのリファレンス
Blenderで面に色を付ける
ogre overlayの使い方
overlay_utils.h (jsk_rviz_plugins)

目次ページへのリンク

ROS講座の目次へのリンク