ROS講座74 twistを送るrviz panel 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の左右でimageを表示したりするウィンドウのことをpanelといいます。このpanelはpluginなので自作することが出来ます。
今回はtwistを送る以下のようなpanelを作成します。

* enableをチェックするとpublishが開始
* Topic欄でTopic名を設定できる。
* チェックをするとTwistStampedになる。
* linear.X、linearYを出す平行移動モードとlinear.X、angular.Z(=Yaw)を出す差動2輪モードを切り替えられる。
* X、Y、Yawの最大値をそれぞれ設定できる。
* タッチ画面でマウスでクリック&ドラッグをすることで値を決める。
* 設定できない項目はグレーアウトする。
* publish rateは10Hで固定

![plugin_twist.png](https://qiita-image-store.s3.amazonaws.com/0/254442/341942c3-35bc-c7c9-0054-8f1de4447fad.png)

# ソースコード

## touch panel widget
タッチすると四角形中の座標を返すというQtのwidgetを記述します。「[72 Qtでタッチパッドを作る](https://qiita.com/srs/items/e336e2b4035a4f9587ab)」で製作したものとほぼ同じものです。
[qt_touch.h](https://github.com/project-srs/ros_lecture/blob/master/plugin_lecture/src/rviz/panel/qt_touch.h)
[qt_touch.cpp](https://github.com/project-srs/ros_lecture/blob/master/plugin_lecture/src/rviz/panel/qt_touch.cpp)

## twist panel

```plugin_lecture/src/qt_twist_panel.h
#ifndef Q_MOC_RUN
#include <ros/ros.h>
#include <rviz/panel.h>
#include <string>
#endif

#include <QLineEdit>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QLabel>
#include <QCheckBox>
#include <QLineEdit>
#include <QRadioButton>
#include <QButtonGroup>
#include <QTimer>

namespace plugin_lecture
{
class TwistPanel : public rviz::Panel
{
  Q_OBJECT
public:
  TwistPanel(QWidget* parent = 0);
  ~TwistPanel();

  virtual void load(const rviz::Config& config);
  virtual void save(rviz::Config config) const;

public Q_SLOTS:
  void tick();

public:
  // The ROS node handle.
  ros::NodeHandle nh_;
  // The ROS publisher for the command velocity.
  ros::Publisher twist_publisher_;

  QCheckBox* enable_check_;
  QLineEdit* topic_edit_;

  QCheckBox* stamped_check_;
  QLineEdit* frame_edit_;

  QRadioButton* radio1_;
  QRadioButton* radio2_;

  QLineEdit* max1_edit_;
  QLineEdit* max2_edit_;
  QLineEdit* max3_edit_;

  TouchWidget* touch_;

  bool pub_stamped_;
  std::string pub_frame_;
};
}  // namespace plugin_lecture
```

* Qtのビルドは少し複雑で、まずMOC(MetaObjectCompile)というものを行います。これはc言語のマクロの展開のようなものです。これとの兼ね合いでincludeのうちQtと関係ないものは`#ifndef Q_MOC_RUN`～`#endif`で囲う必要があります。
* またROSのルールとしてプラグインはpackage名のnamespaceで囲う必要があります。その下ででtwistを送るpluginのクラスを作ります。
* `class qt_twist_panel: public rviz::Panel`のようにrviz::Panelを継承したクラスを作成します。
* 実装は[plugin_lecture/src/qt_twist_panel.cpp](https://github.com/project-srs/ros_lecture/blob/master/plugin_lecture/src/rviz/panel/twist_panel.cpp)で記述します。
    * ここではコンストラクターで画面設定とQtimerの設定をしています。
    * このプログラムではtick()が10Hzで呼ばれます。この中でメインの処理が行われています。
    * save()とload()はrvizの画面設定の保存、読み出しの時に使われる関数です。ここで値の保存読み出しができます。値はQtの型のみが使用可能です(QStringは使えるが、std::Stringは使えない)。
    * setEnabled()関数を使うことで各Widgetの表示をグレーアウト＆不使用にできます。

## 設定の保存
rvizを開いたときにはウィンドウは初期状態で、使うためには毎回値を入れたりしないといけません。しかしrvizの保存機能を使えばrvizを起動したときに***.rvizファイルから設定を読み込むことができます。使い方は簡単で以下のようにsave(), load()関数をクラスに追加するだけです。

```c++:plugin_lecture/src/qt_twist_panel.cppの一部（save、load部分）
void qt_twist_panel::save( rviz::Config config ) const {
  rviz::Panel::save( config );
  config.mapSetValue( "Topic", topic_edit_->text());
  config.mapSetValue( "Stamped", stamped_check_->isChecked());
}

void qt_twist_panel::load( const rviz::Config& config ) {
  rviz::Panel::load( config );
  QString tmp_text;
  bool tmp_bool;
  if( config.mapGetString( "Topic", &tmp_text ))topic_edit_->setText( tmp_text );
  if( config.mapGetBool( "Stamped", &tmp_bool ))stamped_check_->setChecked( tmp_bool );
}
```

## CMakeList.txt
Qt5のリンクのための記述を追加する必要があります。

```cmake:plugin_lecture/CMakeLists.txtに追加
find_package(catkin REQUIRED COMPONENTS
  rviz    #この行を追加
)

set(CMAKE_AUTOMOC ON)
find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED
  Core
  Widgets
)
set(QT_LIBRARIES Qt5::Widgets)
add_definitions(-DQT_NO_KEYWORDS)

add_library(${PROJECT_NAME}
  src/qt_twist_panel.cpp   #この行を追加 
  src/qt_touch.cpp         #この行を追加
)

target_link_libraries(${PROJECT_NAME}
  ${catkin_LIBRARIES}
  ${QT_LIBRARIES}
)
```

## plugin_description.xml

rvizにpanel pluginがあることを通知するためのファイルです。

```xml:plugin_lecture/plugin_description.xml
<library path="lib/libplugin_lecture">
  <class name="plugin_lecture/qt_twist_panel" type="plugin_lecture::qt_twist_panel" base_class_type="rviz::Panel">
    <description>twist panel</description>
  </class>
</library>
```

* `<library>`タグのpath要素ではpluginの静的オブジェクトのパスを指定します。`lib/lib<library名>`と指定します。
* `<class>`タグのname属性の値は自由に決めることが出来ますが`ROSパッケージ名/ライブラリ名`とするのが慣例です。typeは`名前空間/pluginのクラス名`として、base_class_typeは継承している基底クラスの`名前空間/pluginのクラス名`とします。

## package.xml
ビルドの依存と、plugin_description.xmlをrvizにリンクするための記述をします。

```xml:plugin_lecture/package.xml
<package>
  <!-- 中略 -->

  <build_depend>qtbase5-dev</build_depend>
  <build_depend>rviz</build_depend>

  <exec_depend>libqt5-core</exec_depend>
  <exec_depend>libqt5-gui</exec_depend>
  <exec_depend>libqt5-widgets</exec_depend>
  <exec_depend>rviz</exec_depend>

  <export>
    <rviz plugin="${prefix}/plugin_description.xml"/>　　<!-- 追加 -->
  </export>
</package>
```

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

# 実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell:1つ目のウィンドウ
roscore
```

```shell:2つ目のウィンドウ
rviz
```

メニューバーの「Panel」->「add panel」で出てくるウィンドウで「qt_twist_panel」を選択します。
左下に今回作ったpanelが出てきました。

![plugin_twist_rviz.png](https://qiita-image-store.s3.amazonaws.com/0/254442/41aa2984-5d03-6e41-b8a8-aa0b98a6695f.png)

使うとtwistを送信します。

![plugin_twist.gif](https://qiita-image-store.s3.amazonaws.com/0/254442/b6cfb7b9-71d7-7ea7-8d0b-0f0bca02f97b.gif)

# 参考
[rviz_plugin_tutorials](https://github.com/ros-visualization/visualization_tutorials/tree/kinetic-devel/rviz_plugin_tutorials)

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