Help us understand the problem. What is going on with this article?

ROS講座102 pointを送受信するrvizプラグインを作る

環境

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

項目
CPU Core i5-8250U
Ubuntu 16.04
ROS Kinetic

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

概要

今回は2つのRvizのプラグインを制作します。

  • PointStampedを送信するTool plugin
    3Dビューでクリックをすると、その場所に応じたPoseStampedトピックを送信して、その場所にマーカーを置きます。
  • PointStampedを表示するdisplay plugin
    PoseStampedを受信して3Dビューの中に表示します。

Tool plugin

ソースコード

クラス宣言: 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()が発生します。これ以外にキーイベントもとることができます。これの返り血としてはRender(おそらく描画イベントが起きる)、Finished(ツールの選択が解除される)で両方を起こしたいときは|でつなぎます。

ビルド

cd ~/catkin_ws
catkin_make

実行

Rvizを表示して、画面上側のプラスボタンをおしてプラグインを選択します。上部の一覧の中から目的のプラグインを選択して3Dビューの中でクリックします。

plugin_tool.gif

Display plugin

おそらくrvizで一番よく使うのがこのDisplayタイプのプラグインのはずです。

ソースコード

クラス宣言: point_display.h
1つのトピックをSubscribeして何かを表示するということをするだけでしたらclass PointDisplay: public rviz::MessageFilterDisplay<geometry_msgs::PointStamped>のようなMessageFilterDisplayクラスを継承することで簡単に暮らすと生成できます。

クラス実装

point_display.cpp
#include "point_display.h"

#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>

#include <tf/transform_listener.h>
#include <rviz/visualization_manager.h>
#include <rviz/properties/color_property.h>
#include <rviz/properties/float_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/frame_manager.h>
#include <pluginlib/class_list_macros.h>

namespace plugin_lecture{

PointDisplay::PointDisplay(){
  color_property_ = new rviz::ColorProperty( "Color", QColor( 200, 50, 50 ),
                                             "Color to draw the acceleration arrows.",
                                             this, SLOT( updateColorAndAlpha() ));

  alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
                                             "0 is fully transparent, 1.0 is fully opaque.",
                                             this, SLOT( updateColorAndAlpha() ));
}

void PointDisplay::onInitialize(){
  frame_node_ = scene_node_->createChildSceneNode();
  vis_arrow_.reset(new rviz::Arrow( scene_manager_, frame_node_));
  float alpha = alpha_property_->getFloat();
  Ogre::ColourValue color = color_property_->getOgreColor();
  vis_arrow_->setColor(color.r, color.g, color.b, alpha);
  Ogre::Vector3 arrow_scale(0, 0, 0);
  vis_arrow_->setScale(arrow_scale);
  MFDClass::onInitialize(); // MFDClass := MessageFilterDisplay<message type>
}

PointDisplay::~PointDisplay(){
}

void PointDisplay::reset(){
  MFDClass::reset();
}

void PointDisplay::updateColorAndAlpha(){
  float alpha = alpha_property_->getFloat();
  Ogre::ColourValue color = color_property_->getOgreColor();
  vis_arrow_->setColor(color.r, color.g, color.b, alpha);
}

void PointDisplay::updateHistoryLength(){
}

void PointDisplay::processMessage(const geometry_msgs::PointStamped::ConstPtr& msg){
  Ogre::Quaternion orientation;
  Ogre::Vector3 position;
  if( !context_->getFrameManager()->getTransform(msg->header.frame_id, msg->header.stamp, position, orientation)){
    ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
    return;
  }
  frame_node_->setPosition(position);
  frame_node_->setOrientation(orientation);
  Ogre::Vector3 arrow_dir(msg->point.x, msg->point.y, msg->point.z);
  float arrow_length = arrow_dir.length() * 0.77;
  Ogre::Vector3 arrow_scale(arrow_length, arrow_length, arrow_length);
  vis_arrow_->setScale(arrow_scale);
  vis_arrow_->setDirection(arrow_dir);
}

} // namespace plugin_lecture

PLUGINLIB_EXPORT_CLASS(plugin_lecture::PointDisplay,rviz::Display )
  • このクラスの中で3Dビューの中で描画する物と一覧として表示する物の2つを制御する必要があります。
  • リスト表示はrviz::ColorProperty* color_property_;などのような事前に決められたクラスをインスタンス化するだけでリストに追加することができます。

ビルド

cd ~/catkin_ws
catkin_make

実行

Displays画面のAddを押して目的のプラグインを選択します。Displays画面の一覧の中に出てくるので目的のTopicや変数を設定して使います。

plugin_display.gif

参考

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

目次ページへのリンク

ROS講座の目次へのリンク

Why do not you register as a user and use Qiita more conveniently?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away