More than 1 year has passed since last update.

ROS講座74 twistを送るrviz panel pluginを作る

Last updated at 2022-01-21Posted at 2018-10-20

環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

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で固定

ソースコード

touch panel widget

タッチすると四角形中の座標を返すというQtのwidgetを記述します。「72 Qtでタッチパッドを作る」で製作したものとほぼ同じものです。
qt_touch.h
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で記述します。
- ここではコンストラクターで画面設定とQtimerの設定をしています。
- このプログラムではtick()が10Hzで呼ばれます。この中でメインの処理が行われています。
- save()とload()はrvizの画面設定の保存、読み出しの時に使われる関数です。ここで値の保存読み出しができます。値はQtの型のみが使用可能です(QStringは使えるが、std::Stringは使えない)。
- setEnabled()関数を使うことで各Widgetの表示をグレーアウト＆不使用にできます。

設定の保存

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

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のリンクのための記述を追加する必要があります。

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があることを通知するためのファイルです。

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にリンクするための記述をします。

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>