RViz 自定义 Display

背景

RViz 已经内置了许多数据类型的可视化支持。但如果某种消息类型还没有对应的显示插件，可以通过两种方式在 RViz 中查看它：

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

第一种方式会带来额外的网络流量，且在数据表示上有局限性，但胜在快速灵活。第二种方式正是本教程要介绍的——虽然需要多花一些功夫，但能实现更丰富的可视化效果。

本教程的所有代码可以在此仓库中找到。为了方便查看插件的逐步演进过程，该仓库设有不同的分支（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 宏，这样 RViz 的 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 代码。
- library 的 path 是我们将在 CMake 中指定的库名称。
- class 应与上面的 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)
qt6_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>
)
target_link_libraries(point_display PUBLIC
  pluginlib::pluginlib
  rviz_common::rviz_common
  rviz_plugin_tutorial_msgs::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。
- 通过调用 qt6_wrap_cpp 包装每个包含 Q_OBJECT 的头文件。
- 将生成的 MOC_FILES 与其他 cpp 文件一起编入库中。

注意，如果你没有包装头文件，在运行时尝试加载插件时可能会收到类似以下的错误消息：

[ERROR] [1746734178.883047840] [rviz2]: PluginlibFactory: The plugin for class 'Point2D' failed to load. Error: Failed to load library /root/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 dlopen error: /root/ros2_ws/install/rviz_plugin_tutorial/lib/libpoint_display.so: undefined symbol: _ZTVN20rviz_plugin_tutorial12PointDisplayE, at ./src/shared_library.c:96

其余大量代码用于确保插件机制正常运作。其中，调用 pluginlib_export_plugin_description_file 对于让 RViz 找到你的新插件至关重要。

测试运行

编译代码并运行 rviz2。你应该能通过点击左下角的 Add 按钮来添加你的新插件，然后选择你的包/插件。

{/截图：添加 Display 对话框/}

最初，Display 会处于错误状态，因为你还没有指定话题。

{/截图：错误状态/}

如果我们输入话题 /point，它应该能正常加载，但不会显示任何内容。

{/截图：正常运行的空 Display/}

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

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

这条命令会触发 RViz 在 stdout 中输出「We got a message」日志。

实际可视化

你可以通过分支名 step2 查看此步骤的完整版本。

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

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

#include <rviz_rendering/objects/shape.hpp> — rviz_rendering 包中有很多选项可以用来构建可视化。这里我们使用一个简单的形状。
在类中添加一个新的 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_。这样可以确保可视化不会总是相对于固定坐标系出现。
前面一直在搭建的可视化效果就体现在最后四行：将可视化对象的位置设置为消息中的坐标值。

结果应该如下所示：

{/截图：正常运行的 Display/}

如果方框没有出现在预期位置，可能的原因有：

当前没有在发布该话题。
消息在过去 2 秒内没有发布。
你没有在 RViz 中正确设置话题。

添加可配置选项

如果你想让用户自定义可视化的各种属性，需要添加 rviz_common::Property 对象。

你可以通过分支名 step3 查看此步骤的完整版本。

头文件更新

包含颜色属性的头文件：#include <rviz_common/properties/color_property.hpp>。颜色只是你可以设置的众多属性之一。

添加 updateStyle 的声明，该方法在用户通过 Qt 的 SIGNAL/SLOT 机制更改界面属性时被调用：

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

结果应该如下所示：

{/截图：带有颜色属性的 Display/}

哦，粉色！

{/截图：更改颜色后的 Display/}

状态报告

你可以通过分支名 step4 查看此步骤的完整版本。

你还可以设置 Display 的状态。举个例子，让 Display 在 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"），值是状态级别（error/warn/ok）加上描述文字。

{/截图：OK 状态/}

{/截图：警告状态/}

清理美化

接下来做一些清理工作。这一步会让界面更美观、更易用，但并非必须的。你可以通过分支名 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>

我们在 class 标签中添加了 name 字段，它会改变插件在 RViz 中显示的名称。在代码中称其为 PointDisplay 是合理的，但在 RViz 界面中我们想简化显示。
我们在描述中填入了实际文本——不要偷懒。
通过在此处声明特定的消息类型，当你尝试按话题添加 Display 时，RViz 会为该类型的话题自动推荐此插件。

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

我们需要在 CMake 中安装该图片文件：

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

现在当你添加 Display 时，应该能看到图标和描述。

{/截图：添加了图标和描述的 Display 对话框/}

这是按话题添加时的 Display：

{/截图：按话题添加 Display 对话框/}

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

{/截图：标准界面中的图标/}

注意，如果你更改了插件的名称，之前的 RViz 配置将不再生效。