feat(tier4_localization_rviz_plugin): add visualization of pose with covariance history

common/tier4_localization_rviz_plugin/CMakeLists.txt

@@ -15,6 +15,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/pose_history/pose_history_display.cpp
   src/pose_history_footprint/display.hpp
   src/pose_history_footprint/display.cpp
+  src/pose_with_covariance_history/pose_with_covariance_history_display.hpp
+  src/pose_with_covariance_history/pose_with_covariance_history_display.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

common/tier4_localization_rviz_plugin/README.md

@@ -2,20 +2,31 @@
 
 ## Purpose
 
-This plugin can display the history of the localization obtained by ekf_localizer or ndt_scan_matching.
+This plugin can display the localization history obtained by ekf_localizer, ndt_scan_matching, and GNSS.
+If the uncertainty of the estimated pose is given, it can also be displayed.
 
 ## Inputs / Outputs
 
 ### Input
 
-| Name         | Type                              | Description                                                                                    |
-| ------------ | --------------------------------- | ---------------------------------------------------------------------------------------------- |
-| `input/pose` | `geometry_msgs::msg::PoseStamped` | In input/pose, put the result of localization calculated by ekf_localizer or ndt_scan_matching |
+### Pose History
+
+| Name         | Type                              | Description                                                                                           |
+| ------------ | --------------------------------- | ----------------------------------------------------------------------------------------------------- |
+| `input/pose` | `geometry_msgs::msg::PoseStamped` | In input/pose, put the result of localization calculated by ekf_localizer, ndt_scan_matching, or GNSS |
+
+### Pose With Covariance History
+
+| Name                         | Type                                            | Description                                                                                                           |
+| ---------------------------- | ----------------------------------------------- | --------------------------------------------------------------------------------------------------------------------- |
+| `input/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | In input/pose_with_covariance, put the result of localization calculated by ekf_localizer, ndt_scan_matching, or GNSS |
 
 ## Parameters
 
 ### Core Parameters
 
+### Pose History
+
 | Name                    | Type   | Default Value | Description                |
 | ----------------------- | ------ | ------------- | -------------------------- |
 | `property_buffer_size_` | int    | 100           | Buffer size of topic       |
@@ -24,6 +35,26 @@ This plugin can display the history of the localization obtained by ekf_localize
 | `property_line_alpha_`  | float  | 1.0           | Alpha of Line property     |
 | `property_line_color_`  | QColor | Qt::white     | Color of Line property     |
 
+### Pose With Covariance History
+
+| Name                            | Type   | Default Value  | Description                      |
+| ------------------------------- | ------ | -------------- | -------------------------------- |
+| `property_buffer_size_`         | int    | 100            | Buffer size of topic             |
+| `property_path_view_`           | bool   | true           | Use path property or not         |
+| `property_shape_type_`          | string | Line           | Line or Arrow                    |
+| `property_line_width_`          | float  | 0.1            | Width of Line property [m]       |
+| `property_line_alpha_`          | float  | 1.0            | Alpha of Line property           |
+| `property_line_color_`          | QColor | Qt::white      | Color of Line property           |
+| `property_arrow_shaft_length`   | float  | 0.3            | Shaft length of Arrow property   |
+| `property_arrow_shaft_diameter` | float  | 0.15           | Shaft diameter of Arrow property |
+| `property_arrow_head_length`    | float  | 0.2            | Head length of Arrow property    |
+| `property_arrow_head_diameter`  | float  | 0.3            | Head diameter of Arrow property  |
+| `property_arrow_alpha_`         | float  | 1.0            | Alpha of Arrow property          |
+| `property_arrow_color_`         | QColor | Qt::white      | Color of Arrow property          |
+| `property_sphere_scale_`        | float  | 1.0            | Scale of Sphere property         |
+| `property_sphere_alpha_`        | float  | 0.5            | Alpha of Sphere property         |
+| `property_sphere_color_`        | QColor | (204, 51, 204) | Color of Sphere property         |
+
 ## Assumptions / Known limits
 
 TBD.
@@ -32,7 +63,9 @@ TBD.
 
 1. Start rviz and select Add under the Displays panel.
    ![select_add](./images/select_add.png)
-2. Select tier4_localization_rviz_plugin/PoseHistory and press OK.
+2. Select tier4_localization_rviz_plugin/PoseHistory or PoseWithCovarianceHistory. Next, press OK.
    ![select_localization_plugin](./images/select_localization_plugin.png)
-3. Enter the name of the topic where you want to view the trajectory.
+3. Enter the name of the topic where you want to view the trajectory and the covariance.
    ![select_topic_name](./images/select_topic_name.png)
+4. You can view the trajectory and the covariance.
+   ![ex_pose_with_covariance_history](./images/ex_pose_with_covariance_history.png)

common/tier4_localization_rviz_plugin/package.xml

@@ -10,6 +10,7 @@
   <license>Apache License 2.0</license>
   <author email="isamu.takagi@tier4.jp">Takagi, Isamu</author>
   <author email="takamasa.horibe@tier4.jp">Takamasa Horibe</author>
+  <author email="yuhei.oshikubo@tier4.jp">Yuhei Oshikubo</author>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>

common/tier4_localization_rviz_plugin/plugins/plugin_description.xml

@@ -10,4 +10,9 @@
     base_class_type="rviz_common::Display">
     <description>Display footprint of geometry_msgs::Pose</description>
   </class>
+  <class name="rviz_plugins::PoseWithCovarianceHistory"
+    type="rviz_plugins::PoseWithCovarianceHistory"
+    base_class_type="rviz_common::Display">
+    <description>Display geometry_msgs::PoseWithCovarianceStamped</description>
+  </class>
 </library>

common/tier4_localization_rviz_plugin/src/pose_with_covariance_history/pose_with_covariance_history_display.cpp

@@ -0,0 +1,285 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "pose_with_covariance_history_display.hpp"
+
+#include <rviz_common/properties/bool_property.hpp>
+#include <rviz_common/properties/color_property.hpp>
+#include <rviz_common/properties/enum_property.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/int_property.hpp>
+#include <rviz_common/properties/parse_color.hpp>
+#include <rviz_common/validate_floats.hpp>
+#include <rviz_rendering/objects/arrow.hpp>
+#include <rviz_rendering/objects/billboard_line.hpp>
+#include <rviz_rendering/objects/shape.hpp>
+
+#include <tf2/LinearMath/Quaternion.h>
+
+namespace rviz_plugins
+{
+PoseWithCovarianceHistory::PoseWithCovarianceHistory() : last_stamp_(0, 0, RCL_ROS_TIME)
+{
+  property_buffer_size_ = new rviz_common::properties::IntProperty("Buffer Size", 100, "", this);
+
+  property_path_view_ = new rviz_common::properties::BoolProperty("Path", true, "", this);
+  property_shape_type_ = new rviz_common::properties::EnumProperty(
+    "Shape Type", "Line", "", property_path_view_, SLOT(updateShapeType()));
+  property_shape_type_->addOption("Line", 0);
+  property_shape_type_->addOption("Arrow", 1);
+
+  property_line_width_ =
+    new rviz_common::properties::FloatProperty("Width", 0.1, "", property_shape_type_);
+  property_line_alpha_ =
+    new rviz_common::properties::FloatProperty("Alpha", 1.0, "", property_shape_type_);
+  property_line_alpha_->setMin(0.0);
+  property_line_alpha_->setMax(1.0);
+  property_line_color_ =
+    new rviz_common::properties::ColorProperty("Color", Qt::white, "", property_shape_type_);
+
+  property_arrow_shaft_length_ =
+    new rviz_common::properties::FloatProperty("Shaft Length", 0.3, "", property_shape_type_);
+  property_arrow_shaft_diameter_ =
+    new rviz_common::properties::FloatProperty("Shaft diameter", 0.15, "", property_shape_type_);
+  property_arrow_head_length_ =
+    new rviz_common::properties::FloatProperty("Head Length", 0.2, "", property_shape_type_);
+  property_arrow_head_diameter_ =
+    new rviz_common::properties::FloatProperty("Head diameter", 0.3, "", property_shape_type_);
+  property_arrow_alpha_ =
+    new rviz_common::properties::FloatProperty("Alpha", 1.0, "", property_shape_type_);
+  property_arrow_alpha_->setMin(0.0);
+  property_arrow_alpha_->setMax(1.0);
+  property_arrow_color_ =
+    new rviz_common::properties::ColorProperty("Color", Qt::white, "", property_shape_type_);
+
+  property_sphere_view_ = new rviz_common::properties::BoolProperty("Covariance", true, "", this);
+  property_sphere_scale_ =
+    new rviz_common::properties::FloatProperty("Scale", 1.0, "", property_sphere_view_);
+  property_sphere_alpha_ =
+    new rviz_common::properties::FloatProperty("Alpha", 0.5, "", property_sphere_view_);
+  property_sphere_alpha_->setMin(0.0);
+  property_sphere_alpha_->setMax(1.0);
+  property_sphere_color_ = new rviz_common::properties::ColorProperty(
+    "Color", QColor(204, 51, 204), "", property_sphere_view_);
+
+  property_buffer_size_->setMin(0);
+  property_buffer_size_->setMax(10000);
+  property_line_width_->setMin(0.0);
+  property_sphere_scale_->setMin(0.0);
+  property_sphere_scale_->setMax(1000);
+  property_arrow_shaft_length_->setMin(0.0);
+  property_arrow_shaft_diameter_->setMin(0.0);
+  property_arrow_head_length_->setMin(0.0);
+  property_arrow_head_diameter_->setMin(0.0);
+}
+
+PoseWithCovarianceHistory::~PoseWithCovarianceHistory() = default;  // Properties are deleted by Qt
+
+void PoseWithCovarianceHistory::onInitialize()
+{
+  MFDClass::onInitialize();
+  lines_ = std::make_unique<rviz_rendering::BillboardLine>(scene_manager_, scene_node_);
+}
+
+void PoseWithCovarianceHistory::onEnable()
+{
+  subscribe();
+}
+
+void PoseWithCovarianceHistory::onDisable()
+{
+  unsubscribe();
+}
+
+void PoseWithCovarianceHistory::update(float wall_dt, float ros_dt)
+{
+  (void)wall_dt;
+  (void)ros_dt;
+
+  if (!history_.empty()) {
+    lines_->clear();
+    arrows_.clear();
+    spheres_.clear();
+    updateShapeType();
+    updateShapes();
+  }
+}
+
+void PoseWithCovarianceHistory::subscribe()
+{
+  MFDClass::subscribe();
+}
+
+void PoseWithCovarianceHistory::unsubscribe()
+{
+  MFDClass::unsubscribe();
+
+  history_.clear();
+  lines_->clear();
+  arrows_.clear();
+  spheres_.clear();
+}
+
+void PoseWithCovarianceHistory::updateShapeType()
+{
+  bool is_line = property_shape_type_->getOptionInt() == 0;
+  bool is_arrow = property_shape_type_->getOptionInt() == 1;
+
+  property_line_width_->setHidden(!is_line);
+  property_line_alpha_->setHidden(!is_line);
+  property_line_color_->setHidden(!is_line);
+
+  property_arrow_shaft_length_->setHidden(!is_arrow);
+  property_arrow_shaft_diameter_->setHidden(!is_arrow);
+  property_arrow_head_length_->setHidden(!is_arrow);
+  property_arrow_head_diameter_->setHidden(!is_arrow);
+  property_arrow_alpha_->setHidden(!is_arrow);
+  property_arrow_color_->setHidden(!is_arrow);
+}
+
+void PoseWithCovarianceHistory::processMessage(
+  const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr message)
+{
+  if (
+    !rviz_common::validateFloats(message->pose.pose) ||
+    !rviz_common::validateFloats(message->pose.covariance)) {
+    setStatus(
+      rviz_common::properties::StatusProperty::Error, "Topic",
+      "Message contained invalid floating point values (nans or infs)");
+    return;
+  }
+  if (target_frame_ != message->header.frame_id) {
+    history_.clear();
+    spheres_.clear();
+    target_frame_ = message->header.frame_id;
+  }
+  history_.emplace_back(message);
+  last_stamp_ = message->header.stamp;
+  updateHistory();
+}
+
+void PoseWithCovarianceHistory::updateHistory()
+{
+  const auto buffer_size = static_cast<size_t>(property_buffer_size_->getInt());
+  while (buffer_size < history_.size()) {
+    history_.pop_front();
+  }
+}
+
+void PoseWithCovarianceHistory::updateShapes()
+{
+  int shape_type = property_shape_type_->getOptionInt();
+  Ogre::ColourValue color_line =
+    rviz_common::properties::qtToOgre(property_line_color_->getColor());
+  color_line.a = property_line_alpha_->getFloat();
+  Ogre::ColourValue color_sphere =
+    rviz_common::properties::qtToOgre(property_sphere_color_->getColor());
+  color_sphere.a = property_sphere_alpha_->getFloat();
+  Ogre::ColourValue color_arrow =
+    rviz_common::properties::qtToOgre(property_arrow_color_->getColor());
+  color_arrow.a = property_arrow_alpha_->getFloat();
+
+  Ogre::Vector3 line_position;
+  Ogre::Quaternion line_orientation;
+
+  auto frame_manager = context_->getFrameManager();
+  if (!frame_manager->getTransform(target_frame_, last_stamp_, line_position, line_orientation)) {
+    setMissingTransformToFixedFrame(target_frame_);
+    return;
+  }
+
+  setTransformOk();
+  lines_->setMaxPointsPerLine(history_.size());
+  lines_->setLineWidth(property_line_width_->getFloat());
+  lines_->setPosition(line_position);
+  lines_->setOrientation(line_orientation);
+  lines_->setColor(color_line.r, color_line.g, color_line.b, color_line.a);
+
+  while (spheres_.size() < history_.size()) {
+    spheres_.emplace_back(std::make_unique<rviz_rendering::Shape>(
+      rviz_rendering::Shape::Sphere, scene_manager_, scene_node_));
+  }
+  while (arrows_.size() < history_.size()) {
+    arrows_.emplace_back(std::make_unique<rviz_rendering::Arrow>(scene_manager_, scene_node_));
+  }
+
+  for (size_t i = 0; i < history_.size(); ++i) {
+    const auto & message = history_[i];
+
+    Ogre::Vector3 position;
+    position.x = message->pose.pose.position.x;
+    position.y = message->pose.pose.position.y;
+    position.z = message->pose.pose.position.z;
+
+    Ogre::Quaternion orientation;
+    orientation.w = message->pose.pose.orientation.w;
+    orientation.x = message->pose.pose.orientation.x;
+    orientation.y = message->pose.pose.orientation.y;
+    orientation.z = message->pose.pose.orientation.z;
+
+    Eigen::Matrix3d covariance_3d_map;
+    covariance_3d_map(0, 0) = message->pose.covariance[0];
+    covariance_3d_map(0, 1) = message->pose.covariance[1 + 6 * 0];
+    covariance_3d_map(0, 2) = message->pose.covariance[2 + 6 * 0];
+    covariance_3d_map(1, 0) = message->pose.covariance[1 + 6 * 0];
+    covariance_3d_map(1, 1) = message->pose.covariance[1 + 6 * 1];
+    covariance_3d_map(1, 2) = message->pose.covariance[2 + 6 * 1];
+    covariance_3d_map(2, 0) = message->pose.covariance[0 + 6 * 2];
+    covariance_3d_map(2, 1) = message->pose.covariance[1 + 6 * 2];
+    covariance_3d_map(2, 2) = message->pose.covariance[2 + 6 * 2];
+
+    if (property_sphere_view_->getBool()) {
+      Eigen::Matrix3d covariance_3d_base_link;
+      Eigen::Translation3f translation(
+        message->pose.pose.position.x, message->pose.pose.position.y,
+        message->pose.pose.position.z);
+      Eigen::Quaternionf rotation(
+        message->pose.pose.orientation.w, message->pose.pose.orientation.x,
+        message->pose.pose.orientation.y, message->pose.pose.orientation.z);
+      Eigen::Matrix4f pose_matrix4f = (translation * rotation).matrix();
+      const Eigen::Matrix3d rot = pose_matrix4f.topLeftCorner<3, 3>().cast<double>();
+      covariance_3d_base_link = rot.transpose() * covariance_3d_map * rot;
+
+      auto & sphere = spheres_[i];
+      sphere->setPosition(position);
+      sphere->setOrientation(orientation);
+      sphere->setColor(color_sphere.r, color_sphere.g, color_sphere.b, color_sphere.a);
+      sphere->setScale(Ogre::Vector3(
+        property_sphere_scale_->getFloat() * 2 * std::sqrt(covariance_3d_base_link(0, 0)),
+        property_sphere_scale_->getFloat() * 2 * std::sqrt(covariance_3d_base_link(1, 1)),
+        property_sphere_scale_->getFloat() * 2 * std::sqrt(covariance_3d_base_link(2, 2))));
+    }
+
+    if (property_path_view_->getBool()) {
+      if (shape_type == 0) {
+        lines_->addPoint(position);
+      }
+      if (shape_type == 1) {
+        auto & arrow = arrows_[i];
+        arrow->set(
+          property_arrow_shaft_length_->getFloat(), property_arrow_shaft_diameter_->getFloat(),
+          property_arrow_head_length_->getFloat(), property_arrow_head_diameter_->getFloat());
+        arrow->setPosition(position);
+        Ogre::Quaternion y90(Ogre::Degree(-90), Ogre::Vector3::UNIT_Y);
+        arrow->setOrientation(orientation * y90);
+        arrow->setColor(color_arrow.r, color_arrow.g, color_arrow.b, color_arrow.a);
+      }
+    }
+  }
+}
+
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::PoseWithCovarianceHistory, rviz_common::Display)