Go to Qiita Advent Calendar 2024 Top
LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

@srs(project_srs)

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

Posted at

環境

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

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

概要

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

ソースコード

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

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

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を継承します

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
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を見つけるために必要なファイルです。

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>

ビルド&実行

ビルド
source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build
実行
rviz2

rviz_joy_panel.gif

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

目次ページへのリンク

ROS2講座の目次へのリンク

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?