构建自定义 RViz 显示

背景

RViz 中有许多类型的数据具有现有的可视化效果。但是，如果有一种消息类型尚未有插件来显示它，则有两种选择可以在 RViz 中看到它。

将消息转换为另一种类型，例如“visualization_msgs/Marker”。
编写自定义 RViz 显示。

使用第一个选项，网络流量更多，并且数据表示方式受到限制。它也快速而灵活。本教程中解释了后一种选择。它需要一点工作量，但可以带来更丰富的可视化效果。

本教程的所有代码都可以在“此存储库 <https://github.com/MetroRobots/rviz_plugin_tutorial>”中找到。为了查看本教程中编写的插件的增量进度，存储库有不同的分支（“step2”、“step3”……），每个分支都可以随时编译和运行。

Point2D 消息

我们将使用“rviz_plugin_tutorial_msgs”包中定义的玩具消息：“Point2D.msg”：

std_msgs/Header header
float64 x
float64 y

基本插件样板

系紧，代码很多。

您可以使用分支名称“step1”查看此代码的完整版本。

头文件

以下是“point_display.hpp”的内容

#ifndef RVIZ_PLUGIN_TUTORIAL__POINT_DISPLAY_HPP_
#define RVIZ_PLUGIN_TUTORIAL__POINT_DISPLAY_HPP_

#include <rviz_common/message_filter_display.hpp>
#include <rviz_plugin_tutorial_msgs/msg/point2_d.hpp>

namespace rviz_plugin_tutorial
{
class PointDisplay
  : public rviz_common::MessageFilterDisplay<rviz_plugin_tutorial_msgs::msg::Point2D>
{
  Q_OBJECT

protected:
  void processMessage(const rviz_plugin_tutorial_msgs::msg::Point2D::ConstSharedPtr msg) override;
};
}  // namespace rviz_plugin_tutorial

#endif  // RVIZ_PLUGIN_TUTORIAL__POINT_DISPLAY_HPP_

我们正在实现 MessageFilterDisplay 类，该类可用于任何带有 std_msgs/Header 的消息。
该类使用我们的 Point2D 消息类型模板化。
出于超出本教程范围的原因，您需要其中的 Q_OBJECT 宏才能使 GUI 的 QT 部分正常工作。
processMessage 是唯一需要实现的方法，我们将在 cpp 文件中执行此操作。

源文件

point_display.cpp

#include <rviz_plugin_tutorial/point_display.hpp>
#include <rviz_common/logging.hpp>

namespace rviz_plugin_tutorial
{
void PointDisplay::processMessage(const rviz_plugin_tutorial_msgs::msg::Point2D::ConstSharedPtr msg)
{
  RVIZ_COMMON_LOG_INFO_STREAM("We got a message with frame " << msg->header.frame_id);
}
}  // namespace rviz_plugin_tutorial

#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rviz_plugin_tutorial::PointDisplay, rviz_common::Display)

日志记录并非绝对必要，但有助于调试。
为了让 RViz 找到我们的插件，我们需要在代码中调用这个“PLUGINLIB”（以及下面的其他内容）。

package.xml

我们需要在 package.xml 中添加以下三个依赖项：

<depend>pluginlib</depend>
<depend>rviz_common</depend>
<depend>rviz_plugin_tutorial_msgs</depend>

rviz_common_plugins.xml

<library path="point_display">
  <class type="rviz_plugin_tutorial::PointDisplay" base_class_type="rviz_common::Display">
    <description></description>
  </class>
</library>

这是标准的 pluginlib 代码。
库 path 是我们将在 CMake 中分配的库的名称。
该类应与上面的 PLUGINLIB 调用相匹配。
我保证，我们稍后会回到描述。

CMakeLists.txt

将以下几行添加到标准样板的顶部。

find_package(ament_cmake_ros REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rviz_common REQUIRED)
find_package(rviz_plugin_tutorial_msgs REQUIRED)

set(CMAKE_AUTOMOC ON)
qt5_wrap_cpp(MOC_FILES
  include/rviz_plugin_tutorial/point_display.hpp
)

add_library(point_display src/point_display.cpp ${MOC_FILES})
target_include_directories(point_display PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)
ament_target_dependencies(point_display
  pluginlib
  rviz_common
  rviz_plugin_tutorial_msgs
)
install(TARGETS point_display
        EXPORT export_rviz_plugin_tutorial
        ARCHIVE DESTINATION lib
        LIBRARY DESTINATION lib
        RUNTIME DESTINATION bin
)
install(DIRECTORY include/
        DESTINATION include
)
install(FILES rviz_common_plugins.xml
        DESTINATION share/${PROJECT_NAME}
)
ament_export_include_directories(include)
ament_export_targets(export_rviz_plugin_tutorial)
pluginlib_export_plugin_description_file(rviz_common rviz_common_plugins.xml)

要生成正确的 Qt 文件，我们需要
- 打开“CMAKE_AUTOMOC”。
- 通过使用每个包含“Q_OBJECT”的头文件调用“qt5_wrap_cpp”来包装头文件。
- 将“MOC_FILES”与我们的其他 cpp 文件一起包含在库中。

请注意，如果您不包装头文件，则在运行时尝试加载插件时可能会收到一条错误消息，如下所示：

[rviz2]: PluginlibFactory: The plugin for class 'rviz_plugin_tutorial::PointDisplay' failed to load. Error: Failed to load library /home/ros/ros2_ws/install/rviz_plugin_tutorial/lib/libpoint_display.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library LoadLibrary error: /home/ros/ros2_ws/install/rviz_plugin_tutorial/lib/libpoint_display.so: undefined symbol: _ZTVN20rviz_plugin_tutorial12PointDisplayE, at /tmp/binarydeb/ros-foxy-rcutils-1.1.4/src/shared_library.c:84

许多其他代码确保插件部分正常工作。

也就是说，调用“pluginlib_export_plugin_description_file”对于让 RViz 找到您的新插件至关重要。

测试它

编译您的代码并运行“rviz2”。您应该能够通过单击左下角的“添加”来添加您的新插件，然后选择您的包/插件。

最初，显示将处于错误状态，因为您尚未分配主题。

如果我们放入主题“/point”，它应该可以正常加载但不会显示任何内容。

您可以使用以下命令发布消息：

ros2 topic pub /point rviz_plugin_tutorial_msgs/msg/Point2D "{header: {frame_id: map}, x: 1, y: 2}" -r 0.5

这应该会导致“我们收到一条消息”日志出现在 RViz 的“stdout”中。

实际可视化

您可以使用分支名称“step2”查看此步骤的完整版本。

首先，您需要在“CMakeLists.txt”和“package.xml”中添加对包“rviz_rendering”的依赖项。

我们需要在头文件中添加三行：

*“#include <rviz_rendering/objects/shape.hpp>” - rviz_rendering 包<https://github.com/ros2/rviz/tree/ros2/rviz_rendering/include/rviz_rendering/objects>”中有很多选项可用于构建可视化的对象。这里我们使用一个简单的形状。 * 在类中，我们将添加一个新的 protected 虚拟方法：void onInitialize() override; * 我们还添加一个指向形状对象的指针：std::unique_ptr<rviz_rendering::Shape> point_shape_;

然后在 cpp 文件中，我们定义 onInitialize 方法：

void PointDisplay::onInitialize()
{
  MFDClass::onInitialize();
  point_shape_ =
    std::make_unique<rviz_rendering::Shape>(rviz_rendering::Shape::Type::Cube, scene_manager_,
      scene_node_);
}

为方便起见，MFDClass 被别名到模板化的父类。
形状对象必须在 onInitialize 方法中构造，而不是构造函数，否则 scene_manager_ 和 scene_node_ 将无法准备就绪。

我们还更新了我们的 processMessage 方法：

void PointDisplay::processMessage(const rviz_plugin_tutorial_msgs::msg::Point2D::ConstSharedPtr msg)
{
  RVIZ_COMMON_LOG_INFO_STREAM("We got a message with frame " << msg->header.frame_id);

  Ogre::Vector3 position;
  Ogre::Quaternion orientation;
  if (!context_->getFrameManager()->getTransform(msg->header, position, orientation)) {
    RVIZ_COMMON_LOG_DEBUG_STREAM("Error transforming from frame '" << msg->header.frame_id <<
        "' to frame '" << qPrintable(fixed_frame_) << "'");
  }

  scene_node_->setPosition(position);
  scene_node_->setOrientation(orientation);

  Ogre::Vector3 point_pos;
  point_pos.x = msg->x;
  point_pos.y = msg->y;
  point_shape_->setPosition(point_pos);
}

我们需要获取适合消息的正确框架，并相应地转换“scene_node”。

这可确保可视化不会始终相对于固定框架出现。 * 我们一直在构建的实际可视化位于最后四行：我们将可视化的位置设置为与消息的位置相匹配。

结果应如下所示：

如果该框未出现在该位置，则可能是因为：

您此时未发布主题
过去 2 秒内未发布该消息。
您未在 RViz 中正确设置主题。

有选项真好。

如果您想允许用户自定义可视化的不同属性，则需要添加 rviz_common::Property 对象。

您可以使用分支名称 step3 查看此步骤的完整版本。

标题更新

包含颜色属性的头文件：“#include <rviz_common/properties/color_property.hpp>”。颜色只是您可以设置的众多属性之一。添加“updateStyle”的原型，每当通过 Qt 的 SIGNAL/SLOT 框架更改 GUI 时都会调用该原型：

private Q_SLOTS:
  void updateStyle();

添加一个新属性来存储属性本身：std::unique_ptr<rviz_common::properties::ColorProperty> color_property_;

Cpp 更新

#include <rviz_common/properties/parse_color.hpp> - 包含将属性转换为 OGRE 颜色的辅助函数。
在我们的``onInitialize``中添加

color_property_ = std::make_unique<rviz_common::properties::ColorProperty>(
    "Point Color", QColor(36, 64, 142), "Color to draw the point.", this, SLOT(updateStyle()));
updateStyle();

这将使用其名称、默认值、描述和回调构造对象。
我们直接调用“updateStyle”，以便在属性更改之前就开始设置颜色。
然后我们定义回调。

void PointDisplay::updateStyle()
{
  Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_property_->getColor());
  point_shape_->setColor(color);
}

结果应如下所示：

噢，粉红色！

状态报告

您可以使用分支名称“step4”查看此步骤的完整版本。

您还可以设置显示的状态。作为一个任意示例，让我们让显示器在 x 坐标为负时显示警告，因为为什么不呢？在“processMessage”中：

if (msg->x < 0) {
  setStatus(StatusProperty::Warn, "Message",
      "I will complain about points with negative x values.");
} else {
  setStatus(StatusProperty::Ok, "Message", "OK");
}

我们假设先前的 using rviz_common::properties::StatusProperty; 声明。
将状态视为键/值对，其中键是一些字符串（这里我们使用 "Message"），值是状态级别（错误/警告/正常）和描述（其他字符串）。

清理

现在是时候稍微清理一下了。这会让事情看起来更漂亮，使用起来也更容易一些，但并不是严格要求的。您可以使用分支名称“step5”查看此步骤的完整版本。

首先，我们更新插件声明。

<library path="point_display">
  <class name="Point2D" type="rviz_plugin_tutorial::PointDisplay" base_class_type="rviz_common::Display">
    <description>Tutorial to display a point</description>
    <message_type>rviz_plugin_tutorial_msgs/msg/Point2D</message_type>
  </class>
</library>

我们将 name 字段添加到 class 标签。

这会更改 RViz 中显示的名称。在代码中，将其称为 PointDisplay 是有意义的，但在 RViz 中，我们想简化。 * 我们将实际文本放入描述中。不要偷懒。 * 通过在此处声明特定消息类型，当您尝试添加按主题显示时，它将为该类型的主题建议此插件。

我们还在 icons/classes/Point2D.png 为插件添加了一个图标。文件夹是硬编码的，文件名应与插件声明中的名称匹配（如果未指定，则与类的名称匹配）。[icon source]

我们需要在 CMake 中安装图像文件。

install(FILES icons/classes/Point2D.png
        DESTINATION share/${PROJECT_NAME}/icons/classes
)

现在，当您添加显示时，它应该会显示一个图标和说明。

screenshot with added icon and description

这是尝试按主题添加时的显示内容：

最后，这是标准界面中的图标：

screenshot with icon in standard interface

请注意，如果您更改插件名称，以前的 RViz 配置将不再起作用。