Rviz教程（八）：Plugins: New Dockable Panel

TeleopPanel

Overview

This tutorial shows how to write a simple Panel(仪表板) plugin for RViz.

A panel in RViz is a GUI widget(装饰物) which can be docked(使靠码头) in the main window or floating. It does not show properties in the “Displays” panel like a Display, but it could show things in the 3D scene.

A panel can be a useful place to put a bunch(群) of application-specific GUI elements(基础). You could put start and stop buttons for your robot, or other command or control inputs(投入).

RViz has a built-in(嵌入的) tool to send a goal pose(造成) to a path planner, but it does not have a native way to send velocity(速率) commands directly to a robot base controller. That is what this tutorial shows, a subclass(亚纲) of rviz::Panel which lets you send velocity commands right to your robot.

The source code for this tutorial is in the rviz_plugin_tutorials package. You can check out the source directly or (if you use Ubuntu) you can just apt-get install(安装) the pre-compiled Debian package like so:

sudo apt-get install ros-hydro-visualization-tutorials

Here is what RViz looks like with the new “Teleop” panel showing on the left:

The Plugin Code

The code for TeleopPanel is in these files: src/teleop_panel.h, src/teleop_panel.cpp, src/drive_widget.h, and src/drive_widget.cpp.

teleop_panel.h

The full text of teleop_panel(仪表板).h is here: src/teleop_panel.h

Here we declare our new subclass(亚纲) of rviz::Panel. Every panel which can be added via the Panels/Add_New_Panel menu is a subclass of rviz::Panel.

TeleopPanel will show a text-entry field to set the output(输出) topic and a 2D control area. The 2D control area is implemented(实施) by the DriveWidget class, and is described there.

class TeleopPanel: public rviz::Panel
{

This class uses Qt slots(位置) and is a subclass of QObject, so it needs the Q_OBJECT macro(巨大的).

Q_OBJECT
public:

QWidget subclass constructors(构造函数) usually take a parent widget(装饰物) parameter(参数) (which usually defaults to 0). At the same time, pluginlib::ClassLoader creates instances(实例) by calling the default constructor (with no arguments). Taking the parameter and giving a default of 0 lets the default constructor work and also lets someone using the class for something else to pass in a parent widget as they normally would with Qt.

TeleopPanel( QWidget* parent = 0 );

Now we declare overrides(代理佣金) of rviz::Panel functions for saving and loading data from the config(配置) file. Here the data is the topic name.

virtual void load( const rviz::Config& config );
virtual void save( rviz::Config config ) const;

Next come a couple of public Qt slots.

public Q_SLOTS:

The control area, DriveWidget, sends its output to a Qt signal for ease of re-use, so here we declare a Qt slot to receive it.

void setVel( float linear_velocity_, float angular_velocity_ );

In this example setTopic() does not get connected to any signal (it is called directly), but it is easy to define(定义) it as a public slot(位置) instead of a private function in case it would be useful to some other user.

void setTopic( const QString& topic );

Here we declare some internal(内部的) slots.

protected Q_SLOTS:

sendvel() publishes the current velocity(速率) values to a ROS topic. Internally this is connected to a timer which calls it 10 times per second.

void sendVel();

updateTopic() reads the topic name from the QLineEdit and calls setTopic() with the result.

void updateTopic();

Then we finish up with protected member variables(变量).

protected:

The control-area widget(装饰物) which turns mouse events into command velocities.

DriveWidget* drive_widget_;

One-line text editor for entering the outgoing(支出) ROS topic name.

QLineEdit* output_topic_editor_;

The current name of the output(输出) topic.

QString output_topic_;

The ROS publisher for the command velocity.

ros::Publisher velocity_publisher_;

The ROS node handle.

ros::NodeHandle nh_;

The latest velocity values from the drive widget.

float linear_velocity_;
float angular_velocity_;

teleop_panel.cpp

The full text of teleop_panel(仪表板).cpp is here: src/teleop_panel.cpp

Here is the implementation(实现) of the TeleopPanel class. TeleopPanel has these responsibilities(责任):

• Act as a container for GUI elements(元素) DriveWidget and QLineEdit.
• Publish command velocities 10 times per second (whether 0 or not).
• Saving and restoring(恢复) internal state from a config(配置) file.

We start with the constructor(构造函数), doing the standard Qt thing of passing the optional(可选择的) parent argument on to the superclass(超类) constructor(构造函数), and also zero-ing the velocities(速率) we will be publishing.

TeleopPanel::TeleopPanel( QWidget* parent )
  : rviz::Panel( parent )
  , linear_velocity_( 0 )
  , angular_velocity_( 0 )
{

Next we lay out the “output(输出) topic” text entry field using a QLabel and a QLineEdit in a QHBoxLayout.

QHBoxLayout* topic_layout = new QHBoxLayout;
topic_layout->addWidget( new QLabel( "Output Topic:" ));
output_topic_editor_ = new QLineEdit;
topic_layout->addWidget( output_topic_editor_ );

Then create the control widget(装饰物).

drive_widget_ = new DriveWidget;

Lay out the topic field above the control widget.

QVBoxLayout* layout = new QVBoxLayout;
layout->addLayout( topic_layout );
layout->addWidget( drive_widget_ );
setLayout( layout );

Create a timer for sending the output. Motor controllers want to be reassured(安心) frequently that they are doing the right thing, so we keep re-sending velocities even when they aren’t changing.

Here we take advantage of QObject’s memory management behavior(行为): since “this” is passed to the new QTimer as its parent, the QTimer is deleted by the QObject destructor(破坏者) when this TeleopPanel object is destroyed. Therefore we don’t need to keep a pointer to the timer.

QTimer* output_timer = new QTimer( this );

Next we make signal/slot(位置) connections.

connect( drive_widget_, SIGNAL( outputVelocity( float, float )), this, SLOT( setVel( float, float )));
connect( output_topic_editor_, SIGNAL( editingFinished() ), this, SLOT( updateTopic() ));
connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() ));

Start the timer.

output_timer->start( 100 );

Make the control widget start disabled, since we don’t start with an output topic.

  drive_widget_->setEnabled( false );
}

setVel() is connected to the DriveWidget’s output, which is sent whenever it changes due to a mouse event. This just records the values it is given. The data doesn’t actually get sent until the next timer callback(回收).

void TeleopPanel::setVel( float lin, float ang )
{
  linear_velocity_ = lin;
  angular_velocity_ = ang;
}

Read the topic name from the QLineEdit and call setTopic() with the results. This is connected to QLineEdit::editingFinished() which fires when the user presses Enter or Tab or otherwise moves focus away.

void TeleopPanel::updateTopic()
{
  setTopic( output_topic_editor_->text() );
}

Set the topic name we are publishing to.

void TeleopPanel::setTopic( const QString& new_topic )
{

Only take action if the name has changed.

if( new_topic != output_topic_ )
{
  output_topic_ = new_topic;

If the topic is the empty string, don’t publish anything.

if( output_topic_ == "" )
{
  velocity_publisher_.shutdown();
}
else
{

The old velocity_publisher_ is destroyed by this assignment(分配), and thus the old topic advertisement is removed. The call to nh_advertise() says we want to publish data on the new topic name.

  velocity_publisher_ = nh_.advertise<geometry_msgs::Twist>( output_topic_.toStdString(), 1 );
}

rviz::Panel(仪表板) defines(定义) the configChanged() signal. Emitting(发出) it tells RViz that something in this panel has changed that will affect a saved config(配置) file. Ultimately(最终的) this signal can cause QWidget::setWindowModified(true) to be called on the top-level rviz::VisualizationFrame, which causes a little asterisk(星号) (“*”) to show in the window’s title bar indicating(表明)unsaved changes.

  Q_EMIT configChanged();
}

Gray out the control widget when the output topic is empty.

  drive_widget_->setEnabled( output_topic_ != "" );
}

Publish the control velocities(速率) if ROS is not shutting down and the publisher is ready with a valid topic name.

void TeleopPanel::sendVel()
{
  if( ros::ok() && velocity_publisher_ )
  {
    geometry_msgs::Twist msg;
    msg.linear.x = linear_velocity_;
    msg.linear.y = 0;
    msg.linear.z = 0;
    msg.angular.x = 0;
    msg.angular.y = 0;
    msg.angular.z = angular_velocity_;
    velocity_publisher_.publish( msg );
  }
}

Save all configuration(配置) data from this panel(嵌镶板) to the given Config object. It is important here that you call save() on the parent class so the class id and panel name get saved.

void TeleopPanel::save( rviz::Config config ) const
{
  rviz::Panel::save( config );
  config.mapSetValue( "Topic", output_topic_ );
}

Load all configuration data for this panel from the given Config object.

void TeleopPanel::load( const rviz::Config& config )
{
  rviz::Panel::load( config );
  QString topic;
  if( config.mapGetString( "Topic", &topic ))
  {
    output_topic_editor_->setText( topic );
    updateTopic();
  }
}

} // end namespace rviz_plugin_tutorials

Tell pluginlib about this class. Every class which should be loadable(适于承载的) by pluginlib::ClassLoader must have these two lines compiled(编译) in its .cpp file, outside of any namespace scope(范围).

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(rviz_plugin_tutorials::TeleopPanel,rviz::Panel )

drive_widget.h

The full text of drive_widget(装饰物).h is here: src/drive_widget.h

DriveWidget implements(工具) a control which translates mouse Y values into linear(线的) velocities and mouse X values into angular(有角的) velocities.

For maximum reusability(可重用性), this class is only responsible(负责的) for user interaction(相互作用) and display inside its widget. It does not make any ROS or RViz calls. It communicates its data to the outside just via Qt signals.

class DriveWidget: public QWidget
{
Q_OBJECT
public:

This class is not instantiated(例示) by pluginlib::ClassLoader, so the constructor(构造函数) has no restrictions(限制).

DriveWidget( QWidget* parent = 0 );

We override(推翻) QWidget::paintEvent() to do custom painting.

virtual void paintEvent( QPaintEvent* event );

We override the mouse events and leaveEvent() to keep track of what the mouse is doing.

virtual void mouseMoveEvent( QMouseEvent* event );
virtual void mousePressEvent( QMouseEvent* event );
virtual void mouseReleaseEvent( QMouseEvent* event );
virtual void leaveEvent( QEvent* event );

Override sizeHint() to give the layout(布局) managers some idea of a good size for this.

virtual QSize sizeHint() const { return QSize( 150, 150 ); }

We emit(发出) outputVelocity() whenever it changes.

Q_SIGNALS:
  void outputVelocity( float linear, float angular );

mouseMoveEvent() and mousePressEvent() need the same math to figure the velocities, so I put that in here.

protected:
  void sendVelocitiesFromMouse( int x, int y, int width, int height );

A function to emit zero velocity.

void stop();

Finally the member variables(变量):

  float linear_velocity_; // In m/s
  float angular_velocity_; // In radians/s
  float linear_scale_; // In m/s
  float angular_scale_; // In radians/s
};

drive_widget.cpp

The full text of drive_widget(装饰物).cpp is here: src/drive_widget.cpp

The DriveWidget constructor(构造函数) does the normal Qt thing of passing the parent widget to the superclass(超类) constructor, then initializing(初始化) the member variables(变量).

DriveWidget::DriveWidget( QWidget* parent )
  : QWidget( parent )
  , linear_velocity_( 0 )
  , angular_velocity_( 0 )
  , linear_scale_( 10 )
  , angular_scale_( 2 )
{
}

This paintEvent() is complex(复杂的) because of the drawing of the two arc-arrows representing wheel motion(动作). It is not particularly relevant(有关的) to learning how to make an RViz plugin(插件), so I will kind of skim(略读) it.

void DriveWidget::paintEvent( QPaintEvent* event )
{

The background color and crosshair(十字) lines are drawn differently depending on whether this widget is enabled or not. This gives a nice visual indication(指示) of whether the control is “live”.

QColor background;
QColor crosshair;
if( isEnabled() )
{
  background = Qt::white;
  crosshair = Qt::black;
}
else
{
  background = Qt::lightGray;
  crosshair = Qt::darkGray;
}

The main visual is a square, centered in the widget’s area. Here we compute the size of the square and the horizontal(水平的) and vertical(垂直的) offsets(抵消交易) of it.

int w = width();
int h = height();
int size = (( w > h ) ? h : w) - 1;
int hpad = ( w - size ) / 2;
int vpad = ( h - size ) / 2;

QPainter painter( this );
painter.setBrush( background );
painter.setPen( crosshair );

Draw the background square.

painter.drawRect( QRect( hpad, vpad, size, size ));

Draw a cross-hair inside the square.

painter.drawLine( hpad, height() / 2, hpad + size, height() / 2 );
painter.drawLine( width() / 2, vpad, width() / 2, vpad + size );

If the widget is enabled and the velocities(速率) are not zero, draw some sweet green arrows showing possible paths that the wheels of a diff-drive robot would take if it stayed at these velocities.

if( isEnabled() && (angular_velocity_ != 0 || linear_velocity_ != 0 ))
{
  QPen arrow;
  arrow.setWidth( size/20 );
  arrow.setColor( Qt::green );
  arrow.setCapStyle( Qt::RoundCap );
  arrow.setJoinStyle( Qt::RoundJoin );
  painter.setPen( arrow );

This code steps along a central arc defined(定义) by the linear(线的) and angular(有角的) velocites. At each step, it computes where the left and right wheels would be and collects the resulting points in the left_track and right_track arrays(数组).

const int step_count = 100;
QPointF left_track[ step_count ];
QPointF right_track[ step_count ];

float half_track_width = size/4.0;

float cx = w/2;
float cy = h/2;
left_track[ 0 ].setX( cx - half_track_width );
left_track[ 0 ].setY( cy );
right_track[ 0 ].setX( cx + half_track_width );
right_track[ 0 ].setY( cy );
float angle = M_PI/2;
float delta_angle = angular_velocity_ / step_count;
float step_dist = linear_velocity_ * size/2 / linear_scale_ / step_count;
for( int step = 1; step < step_count; step++ )
{
  angle += delta_angle / 2;
  float next_cx = cx + step_dist * cosf( angle );
  float next_cy = cy - step_dist * sinf( angle );
  angle += delta_angle / 2;

  left_track[ step ].setX( next_cx + half_track_width * cosf( angle + M_PI/2 ));
  left_track[ step ].setY( next_cy - half_track_width * sinf( angle + M_PI/2 ));
  right_track[ step ].setX( next_cx + half_track_width * cosf( angle - M_PI/2 ));
  right_track[ step ].setY( next_cy - half_track_width * sinf( angle - M_PI/2 ));

  cx = next_cx;
  cy = next_cy;
}

Now the track arrays are filled, so stroke(抚摸) each with a fat green line.

painter.drawPolyline( left_track, step_count );
painter.drawPolyline( right_track, step_count );

Here we decide which direction each arrowhead(箭头) will point (forward or backward). This works by comparing the arc length travelled(有过…旅行的) by the center in one step (step_dist) with the arc length travelled by the wheel (half_track_width * delta(三角洲)_angle).

int left_arrow_dir = (-step_dist + half_track_width * delta_angle > 0);
int right_arrow_dir = (-step_dist - half_track_width * delta_angle > 0);

Use MiterJoin for the arrowheads so we get a nice sharp point.

arrow.setJoinStyle( Qt::MiterJoin );
painter.setPen( arrow );

Compute and draw polylines(多线) for each arrowhead. This code could probably be more elegant(高雅的).

    float head_len = size / 8.0;
    QPointF arrow_head[ 3 ];
    float x, y;
    if( fabsf( -step_dist + half_track_width * delta_angle ) > .01 )
    {
      x = left_track[ step_count - 1 ].x();
      y = left_track[ step_count - 1 ].y();
      arrow_head[ 0 ].setX( x + head_len * cosf( angle + 3*M_PI/4 + left_arrow_dir * M_PI ));
      arrow_head[ 0 ].setY( y - head_len * sinf( angle + 3*M_PI/4 + left_arrow_dir * M_PI ));
      arrow_head[ 1 ].setX( x );
      arrow_head[ 1 ].setY( y );
      arrow_head[ 2 ].setX( x + head_len * cosf( angle - 3*M_PI/4 + left_arrow_dir * M_PI ));
      arrow_head[ 2 ].setY( y - head_len * sinf( angle - 3*M_PI/4 + left_arrow_dir * M_PI ));
      painter.drawPolyline( arrow_head, 3 );
    }
    if( fabsf( -step_dist - half_track_width * delta_angle ) > .01 )
    {
      x = right_track[ step_count - 1 ].x();
      y = right_track[ step_count - 1 ].y();
      arrow_head[ 0 ].setX( x + head_len * cosf( angle + 3*M_PI/4 + right_arrow_dir * M_PI ));
      arrow_head[ 0 ].setY( y - head_len * sinf( angle + 3*M_PI/4 + right_arrow_dir * M_PI ));
      arrow_head[ 1 ].setX( x );
      arrow_head[ 1 ].setY( y );
      arrow_head[ 2 ].setX( x + head_len * cosf( angle - 3*M_PI/4 + right_arrow_dir * M_PI ));
      arrow_head[ 2 ].setY( y - head_len * sinf( angle - 3*M_PI/4 + right_arrow_dir * M_PI ));
      painter.drawPolyline( arrow_head, 3 );
    }
  }
}

Every mouse move event received here sends a velocity because Qt only sends us mouse move events if there was previously a mouse-press event while in the widget.

void DriveWidget::mouseMoveEvent( QMouseEvent* event )
{
  sendVelocitiesFromMouse( event->x(), event->y(), width(), height() );
}

Mouse-press events should send the velocities too, of course.

void DriveWidget::mousePressEvent( QMouseEvent* event )
{
  sendVelocitiesFromMouse( event->x(), event->y(), width(), height() );
}

When the mouse leaves the widget but the button is still held down, we don’t get the leaveEvent() because the mouse is “grabbed” (by default from Qt). However, when the mouse drags out of the widget and then other buttons are pressed (or possibly other window-manager things happen), we will get a leaveEvent() but not a mouseReleaseEvent(). Without catching this event you can have a robot stuck “on” without the user controlling it.

void DriveWidget::leaveEvent( QEvent* event )
{
  stop();
}

The ordinary way to stop: let go of the mouse button.

void DriveWidget::mouseReleaseEvent( QMouseEvent* event )
{
  stop();
}

Compute and emit(发出) linear and angular velocities based on Y and X mouse positions relative to the central square.

void DriveWidget::sendVelocitiesFromMouse( int x, int y, int width, int height )
{
  int size = (( width > height ) ? height : width );
  int hpad = ( width - size ) / 2;
  int vpad = ( height - size ) / 2;

  linear_velocity_ = (1.0 - float( y - vpad ) / float( size / 2 )) * linear_scale_;
  angular_velocity_ = (1.0 - float( x - hpad ) / float( size / 2 )) * angular_scale_;
  Q_EMIT outputVelocity( linear_velocity_, angular_velocity_ );

update() is a QWidget function which schedules(时间表) this widget to be repainted(重画) the next time through the main event loop(环). We need this because the velocities have just changed, so the arrows need to be redrawn to match.

  update();
}

How to stop: emit(发出) velocities(速率) of 0!

void DriveWidget::stop()
{
  linear_velocity_ = 0;
  angular_velocity_ = 0;
  Q_EMIT outputVelocity( linear_velocity_, angular_velocity_ );
  update();
}

Building the Plugin

To build the plugin(插件), just do the normal “rosmake” thing:

rosmake rviz_plugin_tutorials

Exporting the Plugin

For the plugin to be found and understood by other ROS packages (in this case, rviz), it needs a “plugin_description.xml” file. This file can be named anything you like, as it is specified(指定) in the plugin package’s “manifest(证明).xml” file like so:

<export>
    <rviz plugin="${prefix}/plugin_description.xml"/>
</export>

The contents of plugin_description.xml then look like this:

<library path="lib/librviz_plugin_tutorials">
  <class name="rviz_plugin_tutorials/Teleop"
         type="rviz_plugin_tutorials::TeleopPanel"
         base_class_type="rviz::Panel">
    <description>
      A panel widget allowing simple diff-drive style robot base control.
    </description>
  </class>
  <class name="rviz_plugin_tutorials/Imu"
         type="rviz_plugin_tutorials::ImuDisplay"
         base_class_type="rviz::Display">
    <description>
      Displays direction and scale of accelerations from sensor_msgs/Imu messages.
    </description>
    <message_type>sensor_msgs/Imu</message_type>
  </class>
  <class name="rviz_plugin_tutorials/PlantFlag"
         type="rviz_plugin_tutorials::PlantFlagTool"
         base_class_type="rviz::Tool">
    <description>
      Tool for planting flags on the ground plane in rviz.
    </description>
  </class>
</library>

The first line says that the compiled(编译的) library lives in lib/librviz_plugin_tutorials(个别指导) (the ”.so” ending is appended(附加) by pluginlib according to the OS). This path is relative to the top directory of the package:

<library path="lib/librviz_plugin_tutorials">

The next section is a class entry describing the TeleopPanel:

<class name="rviz_plugin_tutorials/Teleop"
       type="rviz_plugin_tutorials::TeleopPanel"
       base_class_type="rviz::Panel">
  <description>
    A panel widget allowing simple diff-drive style robot base control.
  </description>
</class>

This specifies the name, type, base class, and description of the class. The name field must be a combination(结合) of the first two strings given to the PLUGINLIB_DECLARE_CLASS() macro(巨大的) in the source file. It must be the “package” name, a “/” slash(削减), then the “display name” for the class. The “display name” is the name used for the class in the user interface(界面).

The type entry must be the fully-qualified class name, including any namespace(s) it is inside.

The base_class_type is usually one of rviz::Panel, rviz::Display, rviz::Tool, or rviz::ViewController.

The description subsection(分段) is a simple text description of the class, which is shown in the class-chooser dialog and in the Displays panel(仪表板) help area. This section can contain HTML, including hyperlinks(超链接), but the markup(涨价) must be escaped to avoid being interpreted(说明) as XML markup. For example a link tag might look like: <a href="my-web-page.html">.

Display plugins(插件) can have multiple message_type tags, which are used by RViz when you add a Display by selecting it’s topic first.

Trying It Out

Once your RViz plugin(插件) is compiled(编译) and exported(输出物资), simply run rviz normally:

rosrun rviz rviz

and rviz will use pluginlib to find all the plugins exported to it.

Add a Teleop panel(仪表板) by opening the “Panels” menu and then “Add New Panel” within that. This should bring up a Panel class chooser dialog with “Teleop” in it (here it is “rviz_plugin_tutorials(个别指导)”):

If “Teleop” is not in your list of Panel types, look through RViz’s console(控制台) output(输出) for error messages relating to plugin loading. Some common problems are:

• not having a plugin_description.xml file,
• not exporting it in the manifest(载货单).xml file, or
• not properly referencing(引用) the library file (like librviz_plugin_tutorials.so) from plugin_description.xml.

Once you’ve added the Teleop panel to RViz, you just need to enter a topic name to publish the geometry_msgs/Twist command velocities(速率) on. Once a non-empty string has been entered in the “Output Topic” field, the control square area should light up and accept mouse events. Holding the mouse button down in the control area sends a linear(线的) velocity based on the Y position of the mouse relative to the center and an angular(有角的) velocity based on the X position of the mouse relative to the center.

Next Steps

This Teleop panel might be useful as it is, since it already sends out command velocities appropriate(适当的) for a diff-drive robot. However, there are a few things which might make it more useful:

• Adjustable(可调节的) scaling(缩放比例) of the linear and angular velocities.
• Enforced(实施) maxima(最大数) for the velocities.
• An adjustable robot width parameter(参数), so that the curved(弯曲的) arrows accurately(精确地) show the arc a robot would traverse(穿过).
• A “strafe(猛烈炮轰)” mode (maybe when holding down the Shift key) for robots like the PR2 with (more) holonomic drive ability.