ROS知识：RViz Plugin 教程

ImuDisplay — rviz_plugin_tutorials documentation (ros.org)

RViz Plugin Tutorials — rviz_plugin_tutorials documentation (ros.org)

一、介绍RViz Plugin Tutorials

rviz_plugin_tutorials 包为 rviz 构建了一个插件库，其中包含两个主要类：ImuDisplay 和 TeleopPanel。

ImuDisplay ：这是一个 rviz::Display 子类的示例，允许 rviz 显示来自 sensor_msgs::Imu 消息的数据。
TeleopPanel ：这是一个 rviz::Panel 子类的示例，它显示了用于将速度发送到移动底座的简单控制输入。
PlantFlagTool ：这是一个 rviz::Tool 子类的示例，它允许您在 Z=0 平面上放置标志标记。

二、ImuDisplay 2.1 本教程展示了如何为 RViz 编写一个简单的显示插件。

RViz 目前无法直接显示 sensor_msgs/Imu 消息。本教程中的代码实现了 rviz::Display 的子类来执行此操作。

本教程的源代码位于 rviz_plugin_tutorials 包中。您可以直接查看源代码，或者（如果您使用 Ubuntu）您可以像这样 apt-get 安装预编译的 Debian 软件包：

这是新的 ImuDisplay 输出的样子，显示了来自测试脚本的一系列 sensor_msgs/Imu 消息：

三、Plugin代码

代码ImuDisplay 有下列4个文件: src/imu_display.h, src/imu_display.cpp, src/imu_visual.h, and src/imu_visual.cpp.

3.1 imu_display.h文件

imu_display.h的全文在这里：src/imu_display.h

在这里，我们声明了 rviz::Display 的新子类。可以在“显示”面板中列出的每个显示都是 rviz::Display 的子类。

ImuDisplay 将显示一个 3D 箭头，显示 IMU 加速度矢量的方向和大小。箭头的底部将位于 Imu 消息标题中列出的帧处，箭头的方向将相对于该帧的方向。它还可以选择显示最近的加速度矢量的历史，这些矢量将存储在循环缓冲区中。

ImuDisplay 类本身只实现了循环缓冲区、可编辑参数和 Display 子类机制。视觉效果本身由一个单独的类 ImuVisual 表示。视觉的成语是，当对象存在时，它们出现在场景中，当它们被删除时，它们就消失了。

class ImuDisplay: public rviz::MessageFilterDisplay
{
Q_OBJECT
public:

构造函数。 pluginlib::ClassLoader 通过调用默认构造函数来创建实例，因此请确保您有一个。

ImuDisplay();
virtual ~ImuDisplay();

覆盖来自 Display 的受保护虚拟（C++虚析构函数原理）。尽可能地，当显示未启用时，它们不应订阅传入数据并且不应在 3D 视图中显示任何内容。这些功能是建立和断开这些连接的地方。

protected:
  virtual void onInitialize();

清除此显示回到初始状态的助手。

virtual void reset();

这些 Qt 插槽连接到指示用户可编辑属性更改的信号。

private Q_SLOTS:
  void updateColorAndAlpha();
  void updateHistoryLength();

处理传入 ROS 消息的函数。

private:
  void processMessage( const sensor_msgs::Imu::ConstPtr& msg );

存储视觉对象列表。它是一个循环缓冲区，数据从前面（最旧）弹出并推到后面（最新）

boost::circular_buffer > visuals_;

用户可编辑属性变量：

 rviz::ColorProperty* color_property_;
  rviz::FloatProperty* alpha_property_;
  rviz::IntProperty* history_length_property_;
};

3.2 imu_display.cpp代码¶

imu_display.cpp的全文在这里：src/imu_display.cpp

构造函数必须没有参数，所以我们不能给构造函数完全初始化所需的参数。

ImuDisplay::ImuDisplay()
{
  color_property_ = new rviz::ColorProperty( "Color", QColor( 204, 51, 204 ),
                                             "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() ));

  history_length_property_ = new rviz::IntProperty( "History Length", 1,
                                                    "Number of prior measurements to display.",
                                                    this, SLOT( updateHistoryLength() ));
  history_length_property_->setMin( 1 );
  history_length_property_->setMax( 100000 );
}

在顶层 rviz::Display::initialize() 完成自己的设置后，它调用子类的 onInitialize() 函数。这是我们实例化类的所有工作的地方。我们确保还调用我们的直接超类的 onInitialize() 函数，因为它在设置消息过滤器时做了重要的事情。

注意： “MFDClass” 是一个下列类别对象： MessageFilterDisplay

以节省每次需要引用超类时键入那个长模板化的类名。

void ImuDisplay::onInitialize()
{
  MFDClass::onInitialize();
  updateHistoryLength();
}

ImuDisplay::~ImuDisplay()
{
}

通过删除对象来清除视觉效果。

void ImuDisplay::reset()
{
  MFDClass::reset();
  visuals_.clear();
}

为每个视觉对象设置当前颜色和 alpha 值。

void ImuDisplay::updateColorAndAlpha()
{
  float alpha = alpha_property_->getFloat();
  Ogre::ColourValue color = color_property_->getOgreColor();

  for( size_t i = 0; i < visuals_.size(); i++ )
  {
    visuals_[ i ]->setColor( color.r, color.g, color.b, alpha );
  }
}

设置要显示的过去视觉效果的数量。

void ImuDisplay::updateHistoryLength()
{
  visuals_.rset_capacity(history_length_property_->getInt());
}

这是我们处理传入消息的回调。

void ImuDisplay::processMessage( const sensor_msgs::Imu::ConstPtr& msg )
{

这里我们调用 rviz::FrameManager 来获取从固定帧到这个 Imu 消息头中帧的转换。如果它失败了，我们不能做任何其他事情，所以我们返回。

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;
}

我们正在保持视觉指针的循环缓冲区。这将获取下一个，或者如果缓冲区未满，则创建并存储它

boost::shared_ptr visual;
if( visuals_.full() )
{
  visual = visuals_.front();
}
else
{
  visual.reset(new ImuVisual( context_->getSceneManager(), scene_node_ ));
}

现在设置或更新所选视觉对象的内容。

visual->setMessage( msg );
visual->setFramePosition( position );
visual->setFrameOrientation( orientation );

float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
visual->setColor( color.r, color.g, color.b, alpha );

并将其发送到循环缓冲区的末尾

  visuals_.push_back(visual);
}

} // end namespace rviz_plugin_tutorials

告诉 pluginlib 这个类。在我们包的命名空间之外的全局范围内执行此操作很重要。

#include 
PLUGINLIB_EXPORT_CLASS(rviz_plugin_tutorials::ImuDisplay,rviz::Display )

3.3 imu_visual.h文件

imu_visual.h的全文在这里：src/imu_visual.h 声明此显示的视觉类。

ImuVisual 的每个实例都表示单个 sensor_msgs::Imu 消息的可视化。目前它只显示一个带有加速度矢量方向和大小的箭头，但可以轻松扩展以包含更多消息数据。

class ImuVisual
{
public:

构造函数。创建视觉内容并将其放入场景中，但处于未配置状态。

ImuVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node );

析构函数。从场景中移除视觉内容。

virtual ~ImuVisual();

配置视觉对象以显示消息中的数据。

void setMessage( const sensor_msgs::Imu::ConstPtr& msg );

设置消息所指的坐标系的位姿。这些可以在 setMessage() 中完成，但这需要调用 FrameManager 并在 setMessage() 中进行错误处理，这看起来不太干净。这样ImuVisual只负责可视化。

void setFramePosition( const Ogre::Vector3& position );
void setFrameOrientation( const Ogre::Quaternion& orientation );

设置视觉效果的颜色和 alpha，这是用户可编辑的参数，因此不来自 Imu 消息。

 void setColor( float r, float g, float b, float a );

private:

实现实际箭头形状的对象

boost::shared_ptr acceleration_arrow_;

一个 SceneNode，其位姿设置为匹配 Imu 消息头的坐标系。

Ogre::SceneNode* frame_node_;

SceneManager 保留在这里只是为了让析构函数可以要求它销毁 frame_node_。

 Ogre::SceneManager* scene_manager_;
};

3.4 imu_visual.cpp

imu_visual.cpp的全文在这里：src/imu_visual.cpp

Ogre::SceneNode 形成一棵树，每个节点存储其自身相对于其父节点的变换（位置和方向）。当需要渲染时，Ogre 会计算组合这些变换。

这里我们创建一个节点来存储 Imu 的头帧相对于 RViz 固定帧的位姿。

frame_node_ = parent_node->createChildSceneNode();

我们在框架节点中创建箭头对象，以便我们可以设置它相对于其标题框架的位置和方向。

  acceleration_arrow_.reset(new rviz::Arrow( scene_manager_, frame_node_ ));
}

ImuVisual::~ImuVisual()
{

销毁框架节点，因为我们不再需要它。

  scene_manager_->destroySceneNode( frame_node_ );
}

void ImuVisual::setMessage( const sensor_msgs::Imu::ConstPtr& msg )
{
  const geometry_msgs::Vector3& a = msg->linear_acceleration;

将 geometry_msgs::Vector3 转换为 Ogre::Vector3。

Ogre::Vector3 acc( a.x, a.y, a.z );

求加速度矢量的大小。

float length = acc.length();

在每个维度上缩放箭头的粗细及其长度。

Ogre::Vector3 scale( length, length, length );
acceleration_arrow_->setScale( scale );

设置箭头的方向以匹配加速度矢量的方向。

  acceleration_arrow_->setDirection( acc );
}

位置和方向传递给 SceneNode。

void ImuVisual::setFramePosition( const Ogre::Vector3& position )
{
  frame_node_->setPosition( position );
}

void ImuVisual::setFrameOrientation( const Ogre::Quaternion& orientation )
{
  frame_node_->setOrientation( orientation );
}

颜色被传递到箭头对象。

void ImuVisual::setColor( float r, float g, float b, float a )
{
  acceleration_arrow_->setColor( r, g, b, a );
}

四、构建插件

要构建插件，只需执行正常的“rosmake”操作：

rosmake rviz_plugin_tutorials

五、导出插件

要让其他 ROS 包（在本例中为 rviz）找到并理解该插件，它需要一个“plugin_description.xml”文件。该文件可以任意命名，因为它在插件包的“package.xml”文件中指定，如下所示：

plugin_description.xml 的内容如下所示：


  
    
      A panel widget allowing simple diff-drive style robot base control.
    
  
  
    
      Displays direction and scale of accelerations from sensor_msgs/Imu messages.
    
    sensor_msgs/Imu
  
  
    
      Tool for planting flags on the ground plane in rviz.

第一行表示编译后的库位于 lib/librviz_plugin_tutorials 中（“.so”结尾根据操作系统由 pluginlib 附加）。此路径相对于包的顶级目录：

下一部分是描述 TeleopPanel 的类条目：


  
    A panel widget allowing simple diff-drive style robot base control.

这指定了类的名称、类型、基类和描述。名称字段必须是源文件中提供给 PLUGINLIB_DECLARE_CLASS() 宏的前两个字符串的组合。它必须是“包”名称、“/”斜杠，然后是类的“显示名称”。 “显示名称”是用于用户界面中的类的名称。

类型条目必须是完全限定的类名，包括它所在的任何命名空间。

base_class_type 通常是 rviz::Panel、rviz::Display、rviz::Tool 或 rviz::ViewController 之一。

描述小节是类的简单文本描述，显示在类选择器对话框和显示面板帮助区域中。此部分可以包含 HTML，包括超链接，但必须对标记进行转义以避免被解释为 XML 标记。例如，链接标签可能看起来像：<a href="my-web-page.html">。

显示插件可以有多个 message_type 标签，当您通过首先选择它的主题来添加显示时，RViz 会使用这些标签。

六、试一试

编译并导出 RViz 插件后，只需正常运行 rviz：

rosrun rviz rviz

rviz 将使用 pluginlib 查找导出到它的所有插件。

通过单击“显示”面板底部的“添加”按钮（或通过键入 Control-N）添加 ImuDisplay，然后向下滚动可用的显示，直到在插件包名称下看到“Imu”（这里是 “rviz_plugin_tutorials”）。

与插件加载相关的错误消息。一些常见的问题是：

没有 plugin_description.xml 文件，
不在 package.xml 文件中导出它，或者
未正确引用 plugin_description.xml 中的库文件（如 librviz_plugin_tutorials.so）。
将 Imu 显示添加到 RViz 后，只需将显示的主题名称设置为 sensor_msgs/Imu 消息源。

如果您没有 IMU 或其他 sensor_msgs/Imu 消息来源，您可以使用如下 Python 脚本测试插件：scripts/send_test_msgs.py。

该脚本发布在“/test_imu”主题上，因此请输入该主题。

该脚本会发布 Imu 消息和移动的 TF 帧（“/base_link”相对于“/map”），因此请确保将“Fixed Frame”设置为“/map”。

最后，将 Imu 显示的“History Length”参数调整为 10，您应该会看到类似于本页顶部图片的内容。

注意：如果您使用它来可视化来自实际 IMU 的消息，与大多数机器人相比，箭头会很大：

（注意紫色箭头底部的 PR2 机器人。）这是因为 Imu 加速度单位是米每秒平方，重力是 9.8 m/s^2，我们没有对加速度向量。

七、下一步

这个 ImuDisplay 还不是一个非常有用的 Display 类。使其更有用的扩展可能是：

向加速度矢量添加重力补偿选项。
可视化 Imu 消息中的更多数据。

要添加重力补偿选项，您可能会采取以下步骤：

将新的 rviz::BoolProperty 添加到 ImuDisplay 以存储选项是打开还是关闭。
计算相对于 Imu 框架方向的重力方向（在 ImuVisual::setFrameOrientation() 中设置），并在每次 ImuVisual::setMessage() 中从加速度矢量中减去它。
由于 ImuVisual 将完整的 Imu 消息作为输入，添加更多 Imu 数据的可视化只需要对 ImuVisual 进行修改。

Imu 数据显示可能如下所示：

方向：Imu 参考系上的 rviz::Axes 对象，转动以显示方向。
angular_velocity：可能是一条显示旋转轴的线和一个围绕它弯曲的 3D 箭头以显示旋转速度？
orientation_covariance：也许这是在 X、Y 和 Z 轴末端显示方向的椭圆？
linear_acceleration_covariance：也许这是加速箭头末端的椭球体？

由于所有这些可能在视觉上很混乱，因此包含布尔选项以启用或禁用其中一些可能是有意义的。

ROS知识：RViz Plugin 教程

C/C++/C#相关栏目本月热门文章