ROS講座102 Pointを送信するrviz tool 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)を参照してください。

# 概要
Rvizのtool pluginの作り方を説明します。今回はPointStamped型を送信するpluginを作成します。3Dビューの中でクリックをすると、その場所に応じたPoseStampedトピックを送信して、その場所にマーカーを置きます。

# ソースコード
[クラス宣言: point_tool.h](https://github.com/project-srs/ros_lecture/blob/master/plugin_lecture/src/rviz/tool/point_tool.h)
クラス実装:

```c++:plugin_lecture/src/rviz/tool/point_tool.cpp
#include "point_tool.h"
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreEntity.h>
#include <ros/ros.h>
#include <rviz/viewport_mouse_event.h>
#include <rviz/visualization_manager.h>
#include <rviz/geometry.h>
#include <rviz/ogre_helpers/shape.h>
#include <rviz/frame_manager.h>
#include <geometry_msgs/PointStamped.h>
#include <pluginlib/class_list_macros.h>

namespace plugin_lecture
{
PointTool::PointTool() : nh_()
{
  shortcut_key_ = 'm';
}

PointTool::~PointTool()
{
}

void PointTool::onInitialize()
{
  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);
  vis_shape_->setOrientation(shape_q);
  vis_shape_->setColor(0, 0, 1, 1);
  vis_shape_->getRootNode()->setVisible(false);

  point_pub_ = nh_.advertise<geometry_msgs::PointStamped>("point", 10);
}

void PointTool::activate()
{
  vis_shape_->setColor(0, 0, 1, 1);
  vis_shape_->getRootNode()->setVisible(true);
}

void PointTool::deactivate()
{
  vis_shape_->setColor(0.5, 0.5, 0.5, 1);
}

int PointTool::processMouseEvent(rviz::ViewportMouseEvent& event)
{
  Ogre::Vector3 intersection;
  Ogre::Plane ground_plane(Ogre::Vector3::UNIT_Z, 0.0f);
  if (rviz::getPointOnPlaneFromWindowXY(event.viewport, ground_plane, event.x, event.y, intersection))
  {
    vis_shape_->setPosition(intersection);
    if (event.leftDown())
    {
      geometry_msgs::PointStamped point_msg;
      point_msg.header.frame_id = context_->getFrameManager()->getFixedFrame();
      point_msg.header.stamp = ros::Time::now();
      point_msg.point.x = intersection.x;
      point_msg.point.y = intersection.y;
      point_msg.point.z = intersection.z;
      point_pub_.publish(point_msg);
      return Render | Finished;
    }
  }
  return Render;
}

}  // namespace plugin_lecture
PLUGINLIB_EXPORT_CLASS(plugin_lecture::PointTool, rviz::Tool)
```

* `shortcut_key_ = 'm';`はショートカットキーの登録です。必須ではないですが、登録するとこのキーを押すとこのツールを選択できます。
* 基本的にコンストラクターではなく`onInitialize()`の中で初期化処理を書きます。
* ツールが選択されると（ボタンを押されると）`activate()`が呼ばれ、ツールの選択が解除されると（ほかのツールが呼ばれる等）で`deactivate()`が呼ばれます。
* また3Dビューの上にマウスが来ると`processMouseEvent()`が発生します。これ以外にキーイベントもとることができます。returnの値でrvizに指令を送ることが出来ます。Render(おそらく描画イベントが起きる)、Finished（ツールの選択が解除される）で両方を起こしたいときは`|`でつないだ値をreturnで返します。 
* vis_shape_はRvizの3Dビューの中で円柱を表示するためのクラスです。こちらはOgreのAPIです。[101 ogreで3D描画をする](https://qiita.com/srs/items/9175c43472abe06bda37)でも解説をしています。

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

# 実行
Rvizを表示して、画面上側のプラスボタンをおしてプラグインを選択します。上部の一覧の中から目的のプラグインを選択して3Dビューの中でクリックします。クリックした位置が`/point`というトピックでpublishされます。

![rviz_tool_plugin.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/d5b555ea-701f-eca7-9ac6-40c744bfb5d9.gif)

# 参考
[rviz tool pluginクラスのリファレンス](http://docs.ros.org/jade/api/rviz/html/c++/classrviz_1_1Tool.html)
[rviz_plugin_tutorials](https://github.com/ros-visualization/visualization_tutorials/tree/kinetic-devel/rviz_plugin_tutorials)

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