ROS講座111 Pointを受信するrviz display pluginを作る

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

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

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

# 概要
PointStampedを表示するdisplay pluginを作成します。PoseStampedを受信して3Dビューの中に表示します。
おそらくrvizで一番よく使うのがこのDisplayタイプのプラグインです。

## ソースコード
[クラス宣言: point_display.h](https://github.com/project-srs/ros_lecture/blob/master/plugin_lecture/src/rviz/display/point_display.h)
1つのトピックをSubscribeして何かを表示するということをするだけでしたら`class PointDisplay: public rviz::MessageFilterDisplay<geometry_msgs::PointStamped>`のようなMessageFilterDisplayクラスを継承することで簡単に記述できます。

クラス実装

```c++:point_display.cpp
#include "point_display.h"
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <tf/transform_listener.h>
#include <rviz/visualization_manager.h>
#include <rviz/properties/color_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/frame_manager.h>
#include <pluginlib/class_list_macros.h>

namespace plugin_lecture
{
PointDisplay::PointDisplay()
{
  color_property_ = new rviz::ColorProperty("Color", QColor(200, 50, 50), "Color to draw the acceleration arrows.",
                                            this, SLOT(updateColorAndAlpha()));

  alpha_property_ = new rviz::FloatProperty("Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", this,
                                            SLOT(updateColorAndAlpha()));
}

void PointDisplay::onInitialize()
{
  frame_node_ = scene_node_->createChildSceneNode();
  vis_arrow_.reset(new rviz::Arrow(scene_manager_, frame_node_));
  float alpha = alpha_property_->getFloat();
  Ogre::ColourValue color = color_property_->getOgreColor();
  vis_arrow_->setColor(color.r, color.g, color.b, alpha);
  Ogre::Vector3 arrow_scale(0, 0, 0);
  vis_arrow_->setScale(arrow_scale);
  MFDClass::onInitialize();  // MFDClass := MessageFilterDisplay<message type>
}

PointDisplay::~PointDisplay()
{
}

void PointDisplay::reset()
{
  MFDClass::reset();
}

void PointDisplay::updateColorAndAlpha()
{
  float alpha = alpha_property_->getFloat();
  Ogre::ColourValue color = color_property_->getOgreColor();
  vis_arrow_->setColor(color.r, color.g, color.b, alpha);
}

void PointDisplay::updateHistoryLength()
{
}

void PointDisplay::processMessage(const geometry_msgs::PointStamped::ConstPtr& msg)
{
  Ogre::Quaternion orientation;
  Ogre::Vector3 position;
  if (!context_->getFrameManager()->getTransform(msg->header.frame_id, msg->header.stamp, position, orientation))
  {
    ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(),
              qPrintable(fixed_frame_));
    return;
  }
  frame_node_->setPosition(position);
  frame_node_->setOrientation(orientation);
  Ogre::Vector3 arrow_dir(msg->point.x, msg->point.y, msg->point.z);
  float arrow_length = arrow_dir.length() * 0.77;
  Ogre::Vector3 arrow_scale(arrow_length, arrow_length, arrow_length);
  vis_arrow_->setScale(arrow_scale);
  vis_arrow_->setDirection(arrow_dir);
}

}  // namespace plugin_lecture

PLUGINLIB_EXPORT_CLASS(plugin_lecture::PointDisplay, rviz::Display)
```

* このクラスの中では「3Dビューの中で描画する物」と「設定項目一覧としてパネルに表示する物」の2つについて記述します。
* パネルに表示する物は`rviz::ColorProperty* color_property_;`などのような事前に決められたクラスをインスタンス化するだけで自動的にリストに追加されます。パネル上での値が変更されると、ここで指定したcallback関数が呼ばれます。
* 目的のROSトピックと受信すると`processMessage()`が呼ばれます。ここで3Dビューの中で表示する記述をします。
    * orgeの矢印では軸の長さが指定する長さになり、軸の長さの0.3倍の長さの三角コーンがつきます。長さを約0.77倍にすることで矢印の先までを所望の長さにすることができます。
* また上記の例ではmsgにあるheaderとrvizの「Fixed Frame」が違っていた場合は、「Fixed Frame」->「msgへのheader」のtransformを`context_->getFrameManager()->getTransform()`の関数で取得して描画します。

## ビルド
```shell
cd ~/catkin_ws
catkin_make
```

## 実行
Displays画面のAddを押して目的のプラグインを選択します。Displays画面の一覧の中に出てくるので目的のTopicや変数を設定して使います。
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

![plugin_display.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/f703a71c-6d1a-3aa0-3f56-52dc35275fa1.gif)

# 参考
[rviz display pluginクラスのリファレンス](http://docs.ros.org/jade/api/rviz/html/c++/classrviz_1_1Display.html)

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