Merge branch 'autowarefoundation:main' into feat/distortion_correctio…

…n_node_update_azimuth_and_distance

.github/CODEOWNERS

@@ -77,7 +77,7 @@ evaluator/kinematic_evaluator/** [email protected] fumiya.watanabe@tier4
 evaluator/localization_evaluator/** [email protected] [email protected]
 evaluator/perception_online_evaluator/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 evaluator/planning_evaluator/** [email protected] [email protected]
-evaluator/tier4_metrics_rviz_plugin/** [email protected] [email protected] [email protected]
+evaluator/tier4_metrics_rviz_plugin/** [email protected] [email protected] [email protected] [email protected] [email protected]
 launch/tier4_autoware_api_launch/** [email protected] [email protected]
 launch/tier4_control_launch/** [email protected] [email protected]
 launch/tier4_localization_launch/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -121,7 +121,7 @@ perception/detected_object_feature_remover/** [email protected]
 perception/detected_object_validation/** [email protected] [email protected] [email protected] [email protected]
 perception/detection_by_tracker/** [email protected] [email protected] [email protected]
 perception/elevation_map_loader/** [email protected] [email protected] [email protected]
-perception/euclidean_cluster/** [email protected]
+perception/euclidean_cluster/** [email protected] [email protected]
 perception/ground_segmentation/** [email protected] [email protected] [email protected] [email protected]
 perception/image_projection_based_fusion/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 perception/lidar_apollo_instance_segmentation/** [email protected]
@@ -212,7 +212,7 @@ sensing/image_diagnostics/** [email protected]
 sensing/image_transport_decompressor/** [email protected]
 sensing/imu_corrector/** [email protected] [email protected] [email protected]
 sensing/livox/livox_tag_filter/** [email protected]
-sensing/pointcloud_preprocessor/** [email protected] [email protected] [email protected] [email protected]
+sensing/pointcloud_preprocessor/** [email protected] [email protected] [email protected] [email protected] [email protected]
 sensing/radar_scan_to_pointcloud2/** [email protected] [email protected] [email protected] [email protected]
 sensing/radar_static_pointcloud_filter/** [email protected] [email protected] [email protected] [email protected]
 sensing/radar_threshold_filter/** [email protected] [email protected] [email protected] [email protected]

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -121,7 +121,7 @@ class LaneDepartureChecker
   std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
 
-  std::optional<lanelet::BasicPolygon2d> getFusedLaneletPolygonForPath(
+  std::optional<tier4_autoware_utils::Polygon2d> getFusedLaneletPolygonForPath(
     const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
 
   bool checkPathWillLeaveLane(

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

@@ -321,33 +321,39 @@ std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLanele
     lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
 }
 
-std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
+std::optional<tier4_autoware_utils::Polygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
   const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
 {
   const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
+  auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d {
+    tier4_autoware_utils::Polygon2d p;
+    auto & outer = p.outer();
+
+    for (const auto & p : poly) {
+      tier4_autoware_utils::Point2d p2d(p.x(), p.y());
+      outer.push_back(p2d);
+    }
+    boost::geometry::correct(p);
+    return p;
+  };
+
   // Fuse lanelets into a single BasicPolygon2d
-  auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional<lanelet::BasicPolygon2d> {
+  auto fused_lanelets = [&]() -> std::optional<tier4_autoware_utils::Polygon2d> {
     if (lanelets_distance_pair.empty()) return std::nullopt;
-    if (lanelets_distance_pair.size() == 1)
-      return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
+    tier4_autoware_utils::MultiPolygon2d lanelet_unions;
+    tier4_autoware_utils::MultiPolygon2d result;
 
-    lanelet::BasicPolygon2d merged_polygon =
-      lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
-    for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) {
+    for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) {
       const auto & route_lanelet = lanelets_distance_pair.at(i).second;
-      const auto & poly = route_lanelet.polygon2d().basicPolygon();
-
-      std::vector<lanelet::BasicPolygon2d> lanelet_union_temp;
-      boost::geometry::union_(poly, merged_polygon, lanelet_union_temp);
-
-      // Update merged_polygon by accumulating all merged results
-      merged_polygon.clear();
-      for (const auto & temp_poly : lanelet_union_temp) {
-        merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end());
-      }
+      const auto & p = route_lanelet.polygon2d().basicPolygon();
+      tier4_autoware_utils::Polygon2d poly = to_polygon2d(p);
+      boost::geometry::union_(lanelet_unions, poly, result);
+      lanelet_unions = result;
+      result.clear();
     }
-    if (merged_polygon.empty()) return std::nullopt;
-    return merged_polygon;
+
+    if (lanelet_unions.empty()) return std::nullopt;
+    return lanelet_unions.front();
   }();
 
   return fused_lanelets;

control/mpc_lateral_controller/src/mpc.cpp

@@ -355,12 +355,15 @@ std::pair<bool, VectorXd> MPC::updateStateForDelayCompensation(
   MatrixXd Wd(DIM_X, 1);
   MatrixXd Cd(DIM_Y, DIM_X);
 
+  const double sign_vx = m_is_forward_shift ? 1 : -1;
+
   MatrixXd x_curr = x0_orig;
   double mpc_curr_time = start_time;
   for (size_t i = 0; i < m_input_buffer.size(); ++i) {
     double k, v = 0.0;
     try {
-      k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time);
+      // NOTE: When driving backward, the curvature's sign should be reversed.
+      k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx;
       v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time);
     } catch (const std::exception & e) {
       RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what());
@@ -446,6 +449,7 @@ MPCMatrix MPC::generateMPCMatrix(
     const double ref_vx = reference_trajectory.vx.at(i);
     const double ref_vx_squared = ref_vx * ref_vx;
 
+    // NOTE: When driving backward, the curvature's sign should be reversed.
     const double ref_k = reference_trajectory.k.at(i) * sign_vx;
     const double ref_smooth_k = reference_trajectory.smooth_k.at(i) * sign_vx;
 

evaluator/perception_online_evaluator/package.xml

@@ -21,6 +21,7 @@
   <depend>diagnostic_msgs</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
+  <depend>lanelet2_extension</depend>
   <depend>libgoogle-glog-dev</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>

evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp

@@ -79,7 +79,9 @@ class EvalTest : public ::testing::Test
 
     marker_sub_ = rclcpp::create_subscription<MarkerArray>(
       eval_node, "perception_online_evaluator/markers", 10,
-      [this]([[maybe_unused]] const MarkerArray::SharedPtr msg) { has_received_marker_ = true; });
+      [this]([[maybe_unused]] const MarkerArray::ConstSharedPtr msg) {
+        has_received_marker_ = true;
+      });
     uuid_ = generateUUID();
   }
 

evaluator/tier4_metrics_rviz_plugin/CMakeLists.txt

@@ -8,11 +8,11 @@ find_package(Qt5 REQUIRED Core Widgets Charts)
 set(QT_WIDGETS_LIB Qt5::Widgets)
 set(QT_CHARTS_LIB Qt5::Charts)
 set(CMAKE_AUTOMOC ON)
-set(CMAKE_INCLUDE_CURRENT_DIR ON)
 add_definitions(-DQT_NO_KEYWORDS)
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/metrics_visualize_panel.cpp
+  include/metrics_visualize_panel.hpp
 )
 
 target_link_libraries(${PROJECT_NAME}

evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp → evaluator/tier4_metrics_rviz_plugin/include/metrics_visualize_panel.hpp

@@ -19,11 +19,13 @@
 #ifndef Q_MOC_RUN
 #include <QChartView>
 #include <QColor>
+#include <QComboBox>
 #include <QGridLayout>
 #include <QHeaderView>
 #include <QLabel>
 #include <QLineSeries>
 #include <QPainter>
+#include <QPushButton>
 #include <QTableWidget>
 #include <QVBoxLayout>
 #endif
@@ -33,9 +35,12 @@
 
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 
+#include <iostream>
 #include <limits>
 #include <string>
 #include <unordered_map>
+#include <utility>
+#include <vector>
 
 namespace rviz_plugins
 {
@@ -62,10 +67,10 @@ struct Metric
     {
       auto label = new QLabel;
       label->setAlignment(Qt::AlignCenter);
-      label->setText("metric_name");
+      label->setText(QString::fromStdString(status.name));
       labels.emplace("metric_name", label);
 
-      header.push_back(QString::fromStdString(status.name));
+      header.push_back("metric_name");
     }
 
     for (const auto & [key, value] : status.values) {
@@ -146,6 +151,8 @@ struct Metric
 
   QTableWidget * getTable() const { return table; }
 
+  std::unordered_map<std::string, QLabel *> getLabels() const { return labels; }
+
 private:
   static std::optional<std::string> getValue(const DiagnosticStatus & status, std::string && key)
   {
@@ -186,19 +193,61 @@ class MetricsVisualizePanel : public rviz_common::Panel
   void onInitialize() override;
 
 private Q_SLOTS:
+  // Slot functions triggered by UI events
+  void onTopicChanged();
+  void onSpecificMetricChanged();
+  void onClearButtonClicked();
+  void onTabChanged();
 
 private:
+  // ROS 2 node and subscriptions for handling metrics data
   rclcpp::Node::SharedPtr raw_node_;
   rclcpp::TimerBase::SharedPtr timer_;
-  rclcpp::Subscription<DiagnosticArray>::SharedPtr sub_;
+  std::unordered_map<std::string, rclcpp::Subscription<DiagnosticArray>::SharedPtr> subscriptions_;
+
+  // Topics from which metrics are collected
+  std::vector<std::string> topics_ = {
+    "/diagnostic/planning_evaluator/metrics", "/diagnostic/perception_online_evaluator/metrics"};
 
+  // Timer and metrics message callback
   void onTimer();
-  void onMetrics(const DiagnosticArray::ConstSharedPtr msg);
+  void onMetrics(const DiagnosticArray::ConstSharedPtr & msg, const std::string & topic_name);
+
+  // Functions to update UI based on selected metrics
+  void updateViews();
+  void updateSelectedMetric(const std::string & metric_name);
 
+  // UI components
   QGridLayout * grid_;
+  QComboBox * topic_selector_;
+  QTabWidget * tab_widget_;
+
+  // "Specific Metrics" tab components
+  QComboBox * specific_metric_selector_;
+  QChartView * specific_metric_chart_view_;
+  QTableWidget * specific_metric_table_;
+
+  // Selected metrics data
+  std::optional<std::pair<std::string, Metric>> selected_metrics_;
 
+  // Cache for received messages by topics
+  std::unordered_map<std::string, DiagnosticArray::ConstSharedPtr> current_msg_map_;
+
+  // Mapping from topics to metrics widgets (tables and charts)
+  std::unordered_map<
+    std::string, std::unordered_map<std::string, std::pair<QTableWidget *, QChartView *>>>
+    topic_widgets_map_;
+
+  // Synchronization
   std::mutex mutex_;
+
+  // Stored metrics data
   std::unordered_map<std::string, Metric> metrics_;
+
+  // Utility functions for managing widget visibility based on topics
+  void updateWidgetVisibility(const std::string & target_topic, const bool show);
+  void showCurrentTopicWidgets();
+  void hideInactiveTopicWidgets();
 };
 }  // namespace rviz_plugins
 

evaluator/tier4_metrics_rviz_plugin/package.xml

@@ -7,6 +7,8 @@
   <maintainer email="[email protected]">Satoshi OTA</maintainer>
   <maintainer email="[email protected]">Kyoichi Sugahara</maintainer>
   <maintainer email="[email protected]">Maxime CLEMENT</maintainer>
+  <maintainer email="[email protected]">Kosuke Takeuchi</maintainer>
+  <maintainer email="[email protected]">Fumiya Watanabe</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>