ROS2講座07 Rviz2でタッチのPanelPluginsを作る

# 環境

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

| 項目 |	値 |
|:--|:--|
| CPU | Core i5-8250U |
| Ubuntu | 22.04 |
| ROS2 | Humble |
| Qt | 5.15.3 |

# 概要

デフォルトでも様々なRvizプラグインがありますが、オリジナルで作るのも簡単にできます。

# ソースコード

## C++/Rosハンドルクラス

ROS2の通信をするクラスです。RvizPluginは素直に書くとROSとQtというアプリケーション特有の書き方が強い2つが1つのソースコードに同居してしまいます。ROS2の記述を別クラスに押し込めましょう。

```c++:srs_rviz_plugins/src/joy_panel/joy_handler.hpp
#pragma once

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joy.hpp>

namespace srs_rviz_plugins
{
  class JoyHandler
  {
  public:
    JoyHandler(void) {}

    void setRosNodePtr(const rclcpp::Node::SharedPtr node_ptr)
    {
      node_ptr_ = node_ptr;
    }

    bool initializePublisher(const std::string topic_name)
    {
      if (topic_name == "")
      {
        return false;
      }
      joy_publisher_ = node_ptr_->create_publisher<sensor_msgs::msg::Joy>(topic_name, rclcpp::QoS(10));
      return true;
    }

    void finalizePublisher(void)
    {
      joy_publisher_.reset();
    }

    void publishJoy(std::vector<float> axes, std::vector<int> buttons)
    {
      sensor_msgs::msg::Joy msg{};
      msg.header.stamp = node_ptr_->now();
      msg.axes.resize(2);
      msg.axes = axes;
      msg.buttons = buttons;
      joy_publisher_->publish(msg);
    }

    std::vector<std::string> getTwistTopicList(void) const
    {
      return getTopicList("sensor_msgs/msg/Joy");
    }

  private:
    std::vector<std::string> getTopicList(const std::string type_name) const
    {
      std::map<std::string, std::vector<std::string>> topic_map = node_ptr_->get_topic_names_and_types();

      std::vector<std::string> output;
      for (auto pair : topic_map)
      {
        for (auto s : pair.second)
        {
          if (s == type_name)
          {
            output.push_back(pair.first);
            break;
          }
        }
      }
      return output;
    }

    rclcpp::Node::SharedPtr node_ptr_;
    rclcpp::Publisher<sensor_msgs::msg::Joy>::SharedPtr joy_publisher_;
  };

} // namespace srs_rviz_plugins
```

* `getTopicList()`は特定のmsgtypeのrostopicの名前を抜き出すメソッドです。rclcppのメソッドとして`get_topic_names_and_types()`を呼ぶとそのノードが認識しているrostopicの一覧が取得できます。コードにもあるように外側がROSトピック名のmap、内側がType名のvectorとなっています。

## C++/Plaginクラス

[rviz_common::Panel](https://github.com/ros2/rviz/blob/rolling/rviz_common/include/rviz_common/panel.hpp)を継承します

```c++:srs_rviz_plugins/src/joy_panel/joy_panel.hpp
#pragma once 

#include <QtWidgets>
#include <QComboBox>
#include "touch_widget.hpp"

#ifndef Q_MOC_RUN
#include "joy_handler.hpp"
#include <rviz_common/panel.hpp>
#endif

namespace srs_rviz_plugins
{

class JoyPanel : public rviz_common::Panel
{
  Q_OBJECT
public:
  JoyPanel(QWidget* parent = nullptr);
  void onInitialize() override;
  void load(const rviz_common::Config& config) override;
  void save(rviz_common::Config config) const override;

public Q_SLOTS:
  void onCheckChange(int state);
  void onClickA(void);
  void onClickB(void);
  void onTick(void);
  void onTouchChange(QPointF point);

private:
  void updateTopicList(void);
  
  JoyHandler joy_handler_{};
  QCheckBox* enable_check_;
  QComboBox* topic_combo_;
  TouchWidget* touch_;
  QPushButton* a_button_;
  QPushButton* b_button_;
  QTimer* interval_timer_;
  bool is_active_{false};
  bool a_clicked_{false};
  bool b_clicked_{false};
  QPointF touch_point_{};
};

}  // namespace srs_rviz_plugins
```

```c++:srs_rviz_plugins/src/joy_panel/joy_panel.cpp
#include "joy_panel.hpp"

#include <rviz_common/config.hpp>
#include <rviz_common/display_context.hpp>

#include <QPainter>
#include <QMouseEvent>
#include <QSizePolicy>

namespace srs_rviz_plugins
{
  JoyPanel::JoyPanel(QWidget *parent) : rviz_common::Panel(parent)
  {
    QVBoxLayout *layout = new QVBoxLayout;

    QHBoxLayout *layout_1st = new QHBoxLayout;
    enable_check_ = new QCheckBox("Topic");
    layout_1st->addWidget(enable_check_);
    topic_combo_ = new QComboBox();
    topic_combo_->setEditable(true);
    layout_1st->addWidget(topic_combo_);
    layout->addLayout(layout_1st);

    touch_ = new TouchWidget();
    layout->addWidget(touch_);

    QHBoxLayout *layout_3rd = new QHBoxLayout;
    a_button_ = new QPushButton("A");
    b_button_ = new QPushButton("B");
    layout_3rd->addWidget(a_button_);
    layout_3rd->addWidget(b_button_);
    layout->addLayout(layout_3rd);

    setLayout(layout);

    interval_timer_ = new QTimer(this);

    connect(interval_timer_, &QTimer::timeout, this, &JoyPanel::onTick);
    connect(enable_check_, &QCheckBox::stateChanged, this, &JoyPanel::onCheckChange);
    connect(a_button_, &QPushButton::clicked, this, &JoyPanel::onClickA);
    connect(b_button_, &QPushButton::clicked, this, &JoyPanel::onClickB);
    connect(touch_, qOverload<QPointF>(&TouchWidget::notifyScaledPoint), this, &JoyPanel::onTouchChange);

    interval_timer_->start(100);
    touch_->setEnabled(false);
    touch_->update();
  }

  void JoyPanel::onInitialize()
  {
    joy_handler_.setRosNodePtr(this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node());
    updateTopicList();
  }

  void JoyPanel::onCheckChange(int state)
  {
    if (state == Qt::Checked)
    {
      std::string topic_name = topic_combo_->currentText().toStdString();
      bool ret = joy_handler_.initializePublisher(topic_name);
      if (!ret)
      {
        return;
      }
      topic_combo_->setEnabled(false);
      touch_->setEnabled(true);
      is_active_ = true;
    }
    else
    {
      joy_handler_.finalizePublisher();
      topic_combo_->setEnabled(true);
      touch_->setEnabled(false);
      is_active_ = false;
      updateTopicList();
    }
  }

  void JoyPanel::onClickA()
  {
    a_clicked_ = true;
  }

  void JoyPanel::onClickB()
  {
    b_clicked_ = true;
  }

  void JoyPanel::onTick()
  {
    if (is_active_)
    {
      float axis0 = -touch_point_.y();
      float axis1 = -touch_point_.x();
      joy_handler_.publishJoy({axis0, axis1}, {a_clicked_, b_clicked_});
      a_clicked_ = false;
      b_clicked_ = false;
    }
  }

  void JoyPanel::save(rviz_common::Config config) const
  {
    rviz_common::Panel::save(config);
    config.mapSetValue("BaseTopic", topic_combo_->currentText());
    config.mapSetValue("Checked", enable_check_->isChecked());
  }

  void JoyPanel::load(const rviz_common::Config &config)
  {
    rviz_common::Panel::load(config);
    QString tmp_text;
    bool tmp_bool;
    if (config.mapGetString("BaseTopic", &tmp_text))
    {
      topic_combo_->setCurrentText(tmp_text);
    }
    if (config.mapGetBool("Checked", &tmp_bool))
    {
      enable_check_->setChecked(tmp_bool);
    }
  }

  void JoyPanel::updateTopicList(void)
  {
    std::string previous_topic_name = topic_combo_->currentText().toStdString();
    auto topic_list = joy_handler_.getTwistTopicList();
    topic_combo_->clear();
    int same_topic_index = -1;
    for (auto t : topic_list)
    {
      topic_combo_->addItem(t.c_str());
      if (t == previous_topic_name)
      {
        same_topic_index = topic_combo_->count() - 1;
      }
    }

    if (previous_topic_name != "")
    {
      if (same_topic_index < 0)
      {
        topic_combo_->addItem(previous_topic_name.c_str());
        same_topic_index = topic_combo_->count() - 1;
      }
      topic_combo_->setCurrentIndex(same_topic_index);
    }
  }

  void JoyPanel::onTouchChange(QPointF point)
  {
    touch_point_ = point;
  }

} // namespace srs_rviz_plugins

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(srs_rviz_plugins::JoyPanel, rviz_common::Panel)
```

* `save()`はRvizで設定の保存をするときに呼ばれるメソッドでこの中で設定ファイルへの保存をします。逆に`load()`は起動時に呼ばれるもので、この中で設定をします。逆に言うと、これらの設定を正しく書かないと起動時に想定通りにロードされません。
* `PLUGINLIB_EXPORT_CLASS(srs_rviz_plugins::JoyPanel, rviz_common::Panel)`はプラグインをRvizで読むための設定です。pluginlibのお作法となります。

## plugins_description

Rvizがこのpluginを見つけるために必要なファイルです。

```xml:srs_rviz_plugins/plugins_description.xml
<library path="srs_rviz_plugins">
  <class name="srs_rviz_plugins/JoyPanel" type="srs_rviz_plugins::JoyPanel" base_class_type="rviz_common::Panel">
    <description>joy panel</description>
  </class>
</library>
```

# ビルド&実行

```shell:ビルド
source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build
```

```shell:実行
rviz2
```

![rviz_joy_panel.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/816824d4-d7e7-1036-2163-6ea558e65dcf.gif)

* Rvizのメニューから今回作ったPanelを呼び出します。
* 文字入力欄でtopic名を指定します。（すでにJoyのトピックが存在する場合はその一覧が表示されます）
* チェックを入れると起動します
* rostopic echoで見るとタッチに応じたjoy msgが出力されているのが分かります。

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