feat(planing_evaluator): add dianostic_converter and modfied_goal_deviation

common/tier4_planning_rviz_plugin/CMakeLists.txt

@@ -27,6 +27,8 @@ ament_auto_add_library(tier4_planning_rviz_plugin SHARED
   src/path_with_lane_id/display.cpp
   include/path_with_lane_id_footprint/display.hpp
   src/path_with_lane_id_footprint/display.cpp
+  include/pose_with_uuid_stamped/display.hpp
+  src/pose_with_uuid_stamped/display.cpp
   include/trajectory/display.hpp
   src/trajectory/display.cpp
   include/trajectory_footprint/display.hpp

common/tier4_planning_rviz_plugin/include/pose_with_uuid_stamped/display.hpp

@@ -0,0 +1,84 @@
+// Copyright 2023 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.
+
+#ifndef POSE_WITH_UUID_STAMPED__DISPLAY_HPP_
+#define POSE_WITH_UUID_STAMPED__DISPLAY_HPP_
+
+#include <rviz_common/message_filter_display.hpp>
+
+#include <autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp>
+
+#include <deque>
+#include <memory>
+#include <string>
+
+namespace rviz_rendering
+{
+class Axes;
+class MovableText;
+}  // namespace rviz_rendering
+namespace rviz_common::properties
+{
+class FloatProperty;
+class TfFrameProperty;
+}  // namespace rviz_common::properties
+
+namespace rviz_plugins
+{
+class AutowarePoseWithUuidStampedDisplay
+: public rviz_common::MessageFilterDisplay<autoware_planning_msgs::msg::PoseWithUuidStamped>
+{
+  Q_OBJECT
+
+public:
+  AutowarePoseWithUuidStampedDisplay();
+  ~AutowarePoseWithUuidStampedDisplay() override;
+  AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &) = delete;
+  AutowarePoseWithUuidStampedDisplay(const AutowarePoseWithUuidStampedDisplay &&) = delete;
+  AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &) =
+    delete;
+  AutowarePoseWithUuidStampedDisplay & operator=(const AutowarePoseWithUuidStampedDisplay &&) =
+    delete;
+
+protected:
+  void onInitialize() override;
+  void onEnable() override;
+  void onDisable() override;
+
+private Q_SLOTS:
+  void updateVisualization();
+
+private:
+  void subscribe() override;
+  void unsubscribe() override;
+  void processMessage(
+    const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr meg_ptr) override;
+
+  std::unique_ptr<rviz_rendering::Axes> axes_;
+  std::unique_ptr<Ogre::SceneNode> uuid_node_;
+  rviz_rendering::MovableText * uuid_;
+
+  rviz_common::properties::FloatProperty * length_property_;
+  rviz_common::properties::FloatProperty * radius_property_;
+  rviz_common::properties::TfFrameProperty * frame_property_;
+
+  rviz_common::properties::BoolProperty * uuid_text_view_property_;
+  rviz_common::properties::FloatProperty * uuid_text_scale_property_;
+
+  autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr last_msg_ptr_;
+};
+
+}  // namespace rviz_plugins
+
+#endif  // POSE_WITH_UUID_STAMPED__DISPLAY_HPP_

common/tier4_planning_rviz_plugin/package.xml

@@ -13,6 +13,7 @@
   <build_depend>autoware_cmake</build_depend>
 
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>

common/tier4_planning_rviz_plugin/plugins/plugin_description.xml

@@ -24,6 +24,11 @@
     base_class_type="rviz_common::Display">
     <description>Display footprint of autoware_auto_planning_msg::PathWithLaneId</description>
   </class>
+  <class name="rviz_plugins/PoseWithUuidStamped"
+    type="rviz_plugins::AutowarePoseWithUuidStampedDisplay"
+    base_class_type="rviz_common::Display">
+    <description>Display pose_with_uuid_stamped of autoware_planning_msg::PoseWithUuidStamped</description>
+  </class>
   <class name="rviz_plugins/Trajectory"
     type="rviz_plugins::AutowareTrajectoryDisplay"
     base_class_type="rviz_common::Display">

common/tier4_planning_rviz_plugin/src/pose_with_uuid_stamped/display.cpp

@@ -0,0 +1,146 @@
+// Copyright 2023 TIER IV, Inc. All rights reserved.
+//
+// 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 "rviz_common/properties/tf_frame_property.hpp"
+
+#include <pose_with_uuid_stamped/display.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/validate_floats.hpp>
+#include <rviz_rendering/objects/axes.hpp>
+#include <rviz_rendering/objects/movable_text.hpp>
+
+namespace
+{
+std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u)
+{
+  std::stringstream ss;
+  for (auto i = 0; i < 16; ++i) {
+    ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i];
+  }
+  return ss.str();
+}
+}  // namespace
+
+namespace rviz_plugins
+{
+AutowarePoseWithUuidStampedDisplay::AutowarePoseWithUuidStampedDisplay()
+{
+  length_property_ = new rviz_common::properties::FloatProperty(
+    "Length", 1.5f, "Length of each axis, in meters.", this, SLOT(updateVisualization()));
+  length_property_->setMin(0.0001f);
+
+  radius_property_ = new rviz_common::properties::FloatProperty(
+    "Radius", 0.5f, "Radius of each axis, in meters.", this, SLOT(updateVisualization()));
+  radius_property_->setMin(0.0001f);
+
+  uuid_text_view_property_ = new rviz_common::properties::BoolProperty(
+    "UUID", false, "flag of visualizing uuid text", this, SLOT(updateVisualization()), this);
+  uuid_text_scale_property_ = new rviz_common::properties::FloatProperty(
+    "Scale", 0.3f, "Scale of uuid text", uuid_text_view_property_, SLOT(updateVisualization()),
+    this);
+}
+
+AutowarePoseWithUuidStampedDisplay::~AutowarePoseWithUuidStampedDisplay() = default;
+
+void AutowarePoseWithUuidStampedDisplay::onInitialize()
+{
+  MFDClass::onInitialize();
+  axes_ = std::make_unique<rviz_rendering::Axes>(
+    scene_manager_, scene_node_, length_property_->getFloat(), radius_property_->getFloat());
+  axes_->getSceneNode()->setVisible(isEnabled());
+
+  uuid_node_.reset(scene_node_->createChildSceneNode());
+  uuid_ = new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1);
+}
+
+void AutowarePoseWithUuidStampedDisplay::onEnable()
+{
+  subscribe();
+  axes_->getSceneNode()->setVisible(true);
+}
+
+void AutowarePoseWithUuidStampedDisplay::onDisable()
+{
+  unsubscribe();
+  axes_->getSceneNode()->setVisible(false);
+}
+
+void AutowarePoseWithUuidStampedDisplay::subscribe() { MFDClass::subscribe(); }
+
+void AutowarePoseWithUuidStampedDisplay::unsubscribe() { MFDClass::unsubscribe(); }
+
+void AutowarePoseWithUuidStampedDisplay::updateVisualization()
+{
+  if (last_msg_ptr_ != nullptr) {
+    processMessage(last_msg_ptr_);
+  }
+}
+
+void AutowarePoseWithUuidStampedDisplay::processMessage(
+  const autoware_planning_msgs::msg::PoseWithUuidStamped::ConstSharedPtr msg_ptr)
+{
+  uuid_node_->detachAllObjects();
+
+  {
+    Ogre::Vector3 position;
+    Ogre::Quaternion orientation;
+
+    if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) {
+      const auto frame = msg_ptr->header.frame_id.c_str();
+      RCLCPP_DEBUG(
+        rclcpp::get_logger("AutowarePoseWithUuidStampedDisplay"),
+        "Error transforming from frame '%s' to frame '%s'", frame, qPrintable(fixed_frame_));
+      axes_->getSceneNode()->setVisible(false);
+      uuid_->setVisible(false);
+      setMissingTransformToFixedFrame(frame);
+    } else {
+      setTransformOk();
+      axes_->getSceneNode()->setVisible(true);
+      uuid_->setVisible(true);
+      scene_node_->setPosition(position);
+      scene_node_->setOrientation(orientation);
+    }
+  }
+
+  {
+    Ogre::Vector3 position;
+    position.x = msg_ptr->pose.position.x;
+    position.y = msg_ptr->pose.position.y;
+    position.z = msg_ptr->pose.position.z;
+
+    Ogre::Quaternion orientation;
+    orientation.x = msg_ptr->pose.orientation.x;
+    orientation.y = msg_ptr->pose.orientation.y;
+    orientation.z = msg_ptr->pose.orientation.z;
+    orientation.w = msg_ptr->pose.orientation.w;
+    axes_->setPosition(position);
+    axes_->setOrientation(orientation);
+    axes_->set(length_property_->getFloat(), radius_property_->getFloat());
+
+    if (uuid_text_view_property_->getBool()) {
+      uuid_node_->setPosition(position);
+      uuid_->setTextAlignment(
+        rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE);
+      uuid_->setCaption(uuid_to_string(msg_ptr->uuid));
+      uuid_->setCharacterHeight(uuid_text_scale_property_->getFloat());
+      uuid_->setVisible(true);
+      uuid_node_->attachObject(uuid_);
+    }
+  }
+  last_msg_ptr_ = msg_ptr;
+}
+}  // namespace rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePoseWithUuidStampedDisplay, rviz_common::Display)

evaluator/diagnostic_converter/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.16.3) # Ubuntu 20.04 default CMake version
+
+project(diagnostic_converter)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+find_package(pluginlib REQUIRED)
+
+ament_auto_find_build_dependencies()
+
+
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
+  src/converter_node.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_node
+  PLUGIN "diagnostic_converter::DiagnosticConverter"
+  EXECUTABLE ${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(test_${PROJECT_NAME}
+    test/test_converter_node.cpp
+  )
+  target_link_libraries(test_${PROJECT_NAME}
+    ${PROJECT_NAME}_node
+  )
+endif()
+
+ament_auto_package()

evaluator/diagnostic_converter/README.md

@@ -0,0 +1,53 @@
+# Planning Evaluator
+
+## Purpose
+
+This package provides a node to convert `diagnostic_msgs::msg::DiagnosticArray` messages
+into `tier4_simulation_msgs::msg::UserDefinedValue` messages.
+
+## Inner-workings / Algorithms
+
+The node subscribes to all topics listed in the parameters and assumes they publish
+`DiagnosticArray` messages.
+Each time such message is received,
+it is converted into as many `UserDefinedValue` messages as the number of `KeyValue` objects.
+The format of the output topic is detailed in the _output_ section.
+
+## Inputs / Outputs
+
+### Inputs
+
+The node listens to `DiagnosticArray` messages on the topics specified in the parameters.
+
+### Outputs
+
+The node outputs `UserDefinedValue` messages that are converted from the received `DiagnosticArray`.
+
+The name of the output topics are generated from the corresponding input topic, the name of the diagnostic status, and the key of the diagnostic.
+For example, we might listen to topic `/diagnostic_topic` and receive a `DiagnosticArray` with 2 status:
+
+- Status with `name: "x"`.
+  - Key: `a`.
+  - Key: `b`.
+- Status with `name: "y"`.
+  - Key: `a`.
+  - Key: `c`.
+
+The resulting topics to publish the `UserDefinedValue` are as follows:
+
+- `/metrics_x_a`.
+- `/metrics_x_b`.
+- `/metrics_y_a`.
+- `/metrics_y_c`.
+
+## Parameters
+
+| Name                | Type             | Description                                                   |
+| :------------------ | :--------------- | :------------------------------------------------------------ |
+| `diagnostic_topics` | list of `string` | list of DiagnosticArray topics to convert to UserDefinedValue |
+
+## Assumptions / Known limits
+
+Values in the `KeyValue` objects of a `DiagnosticStatus` are assumed to be of type `double`.
+
+## Future extensions / Unimplemented parts