ROS講座102 Pointを送信するrviz tool pluginを作る

環境

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

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

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

概要

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

ソースコード

クラス宣言: point_tool.h
クラス実装:

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描画をするでも解説をしています。

ビルド

cd ~/catkin_ws
catkin_make

実行

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

参考

rviz tool pluginクラスのリファレンス
rviz_plugin_tutorials

目次ページへのリンク

ROS講座の目次へのリンク