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

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

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

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

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

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


# ソースコード
[ノード本体:qt_lecture/src/render/qt_basic5.cpp](https://github.com/project-srs/ros_lecture/blob/master/qt_lecture/src/render/qt_basic5.cpp)
[ライブラリ宣言：qt_lecture/src/render/qt_render.h](https://github.com/project-srs/ros_lecture/blob/master/qt_lecture/src/render/qt_render.h)

ライブラリ実装

```c++: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オブジェクトの階層構造になっています。

# ビルド
```shell
cd ~/catkin_ws
catkin build
```

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

```shell:1つ目のターミナル
roscore
```

```shell:1つ目のターミナル
rosrun qt_lecture qt_basic5 
```

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

![qt_render.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/dcb03b33-a77a-3242-c153-be04daeca6b3.png)

# 参考
[visualization_tutorialソースコード](https://github.com/ros-visualization/visualization_tutorials/tree/kinetic-devel/rviz_plugin_tutorials)
[rviz/Objectのリファレンス](http://docs.ros.org/melodic/api/rviz/html/c++/classrviz_1_1Object.html)
[Blenderで面に色を付ける](https://neo-sahara.com/wp/2018/06/23/blender-2-79-%E9%9D%A2%E3%81%94%E3%81%A8%E3%81%AB%E5%88%A5%E3%81%AE%E8%89%B2%E3%82%92%E3%81%A4%E3%81%91%E3%82%8B%E3%80%82/)
[ogre overlayの使い方](http://mogure.yokochou.com/ogretut01.html)
[overlay_utils.h (jsk_rviz_plugins)](https://github.com/jsk-ros-pkg/jsk_visualization/blob/master/jsk_rviz_plugins/src/overlay_utils.h)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)