Merge branch 'main' into feat-configurable-qos

Author: ralwing

Diff for: .github/workflows/build-and-test-daily-arm64.yaml

@@ -73,6 +73,7 @@ jobs:
           fail_ci_if_error: false
           verbose: true
           flags: total-arm64
+          token: ${{ secrets.CODECOV_TOKEN }}
 
       - name: Show disk space after the tasks
         run: df -h

Diff for: .github/workflows/build-and-test-daily.yaml

@@ -73,6 +73,7 @@ jobs:
           fail_ci_if_error: false
           verbose: true
           flags: total
+          token: ${{ secrets.CODECOV_TOKEN }}
 
       - name: Show disk space after the tasks
         run: df -h

Diff for: .github/workflows/build-and-test-differential-arm64.yaml

@@ -87,6 +87,7 @@ jobs:
           fail_ci_if_error: false
           verbose: true
           flags: differential-arm64
+          token: ${{ secrets.CODECOV_TOKEN }}
 
       - name: Show disk space after the tasks
         run: df -h

Diff for: .github/workflows/build-and-test-differential.yaml

@@ -114,6 +114,7 @@ jobs:
           fail_ci_if_error: false
           verbose: true
           flags: differential
+          token: ${{ secrets.CODECOV_TOKEN }}
 
       - name: Show disk space after the tasks
         run: df -h

Diff for: .github/workflows/build-and-test.yaml

@@ -115,6 +115,7 @@ jobs:
           fail_ci_if_error: false
           verbose: true
           flags: total
+          token: ${{ secrets.CODECOV_TOKEN }}
 
       - name: Show disk space after the tasks
         run: df -h

Diff for: .github/workflows/cppcheck-daily.yaml

@@ -22,7 +22,7 @@ jobs:
         continue-on-error: true
         id: cppcheck
         run: |
-          cppcheck --enable=all --inconclusive --check-level=exhaustive --suppress=*:*/test/* --error-exitcode=1 --xml . 2> cppcheck-report.xml
+          cppcheck --enable=all --inconclusive --check-level=exhaustive --suppress=*:*/test/* --error-exitcode=1 --xml --inline-suppr . 2> cppcheck-report.xml
         shell: bash
 
       - name: Count errors by error ID and severity

Diff for: .github/workflows/cppcheck-differential.yaml

@@ -22,6 +22,13 @@ jobs:
           sudo apt-get update
           sudo apt-get install -y git
 
+      - name: Install colcon
+        run: |
+          sudo sh -c 'echo "deb [arch=amd64,arm64] http://repo.ros2.org/ubuntu/main `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list'
+          curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
+          sudo apt update
+          sudo apt install python3-colcon-common-extensions
+
       # cppcheck from apt does not yet support --check-level args, and thus install from snap
       - name: Install Cppcheck from snap
         run: |
@@ -31,33 +38,42 @@ jobs:
         run: git fetch origin ${{ github.base_ref }}
         shell: bash
 
-      - name: Get changed files (existing files only)
-        id: get-changed-files
+      - name: Get modified packages
+        id: get-modified-packages
+        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
+
+      - name: Get full paths of modified packages
+        id: get-full-paths
         run: |
-          echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT
-        shell: bash
+          modified_packages="${{ steps.get-modified-packages.outputs.modified-packages }}"
+          paths=""
+          for pkg in $modified_packages; do
+            path=$(colcon list --packages-select $pkg | awk '{print $2}' | xargs realpath)
+            paths="$paths $path"
+          done
+          echo "full-paths=$paths" >> $GITHUB_OUTPUT
 
-      - name: Run Cppcheck on changed files
-        if: ${{ steps.get-changed-files.outputs.changed-files != '' }}
+      - name: Run Cppcheck on modified packages
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
         continue-on-error: true
         id: cppcheck
         run: |
-          echo "Running Cppcheck on changed files: ${{ steps.get-changed-files.outputs.changed-files }}"
-          cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions ${{ steps.get-changed-files.outputs.changed-files }} 2> cppcheck-report.txt
+          echo "Running Cppcheck on modified packages: ${{ steps.get-modified-packages.outputs.modified-packages }}"
+          cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions --inline-suppr ${{ steps.get-full-paths.outputs.full-paths }} 2> cppcheck-report.txt
         shell: bash
 
       - name: Show cppcheck-report result
-        if: ${{ steps.get-changed-files.outputs.changed-files != '' }}
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
         run: |
           cat cppcheck-report.txt
 
       - name: Upload Cppcheck report
-        if: ${{ steps.get-changed-files.outputs.changed-files != '' }}
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
         uses: actions/upload-artifact@v2
         with:
           name: cppcheck-report
           path: cppcheck-report.txt
 
       - name: Fail the job if Cppcheck failed
-        if: ${{ steps.get-changed-files.outputs.changed-files != '' && steps.cppcheck.outcome == 'failure' }}
+        if: ${{ steps.get-modified-packages.outputs.modified-packages != '' && steps.cppcheck.outcome == 'failure' }}
         run: exit 1

Diff for: common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/assets/images/wheel.png

@@ -76,6 +76,7 @@ private Q_SLOTS:
   rviz_common::properties::IntProperty * property_left_;
   rviz_common::properties::IntProperty * property_top_;
   rviz_common::properties::ColorProperty * property_signal_color_;
+  rviz_common::properties::FloatProperty * property_handle_angle_scale_;
   std::unique_ptr<rviz_common::properties::RosTopicProperty> steering_topic_property_;
   std::unique_ptr<rviz_common::properties::RosTopicProperty> gear_topic_property_;
   std::unique_ptr<rviz_common::properties::RosTopicProperty> speed_topic_property_;

Diff for: common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp

@@ -36,7 +36,8 @@ class SteeringWheelDisplay
 {
 public:
   SteeringWheelDisplay();
-  void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect);
+  void drawSteeringWheel(
+    QPainter & painter, const QRectF & backgroundRect, float handle_angle_scale_);
   void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg);
 
 private:
@@ -46,6 +47,7 @@ class SteeringWheelDisplay
   QImage wheelImage;
   QImage scaledWheelImage;
   QImage coloredImage(const QImage & source, const QColor & color);
+  autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_;
 };
 
 }  // namespace autoware_overlay_rviz_plugin

Diff for: common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp

@@ -49,6 +49,9 @@ SignalDisplay::SignalDisplay()
   property_signal_color_ = new rviz_common::properties::ColorProperty(
     "Signal Color", QColor(QString("#00E678")), "Color of the signal arrows", this,
     SLOT(updateOverlayColor()));
+  property_handle_angle_scale_ = new rviz_common::properties::FloatProperty(
+    "Handle Angle Scale", 17.0, "Scale of the steering wheel handle angle", this,
+    SLOT(updateOverlaySize()));
 
   // Initialize the component displays
   steering_wheel_display_ = std::make_unique<SteeringWheelDisplay>();
@@ -285,7 +288,8 @@ void SignalDisplay::drawWidget(QImage & hud)
   }
 
   if (steering_wheel_display_) {
-    steering_wheel_display_->drawSteeringWheel(painter, backgroundRect);
+    steering_wheel_display_->drawSteeringWheel(
+      painter, backgroundRect, property_handle_angle_scale_->getFloat());
   }
 
   if (speed_display_) {

Diff for: common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp

@@ -59,19 +59,17 @@ void SteeringWheelDisplay::updateSteeringData(
   const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg)
 {
   try {
-    // Assuming msg->steering_angle is the field you're interested in
-    float steeringAngle = msg->steering_tire_angle;
-    // we received it as a radian value, but we want to display it in degrees
-    steering_angle_ =
-      (steeringAngle * -180 / M_PI) *
-      17;  // 17 is the ratio between the steering wheel and the steering tire angle i assume
+    last_msg_ptr_ = msg;
+
+    steering_angle_ = msg->steering_tire_angle;
   } catch (const std::exception & e) {
     // Log the error
     std::cerr << "Error in processMessage: " << e.what() << std::endl;
   }
 }
 
-void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect)
+void SteeringWheelDisplay::drawSteeringWheel(
+  QPainter & painter, const QRectF & backgroundRect, float handle_angle_scale_)
 {
   // Enable Antialiasing for smoother drawing
   painter.setRenderHint(QPainter::Antialiasing, true);
@@ -80,7 +78,7 @@ void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF &
   QImage wheel = coloredImage(scaledWheelImage, gray);
 
   // Rotate the wheel
-  float steeringAngle = steering_angle_;  // No need to round here
+  float steeringAngle = std::round(handle_angle_scale_ * (steering_angle_ / M_PI) * -180.0);
   // Calculate the position
   int wheelCenterX = backgroundRect.left() + wheel.width() + 54 + 54;
   int wheelCenterY = backgroundRect.height() / 2;
@@ -99,16 +97,40 @@ void SteeringWheelDisplay::drawSteeringWheel(QPainter & painter, const QRectF &
   // Draw the rotated image
   painter.drawImage(drawPoint.x(), drawPoint.y(), rotatedWheel);
 
-  QString steeringAngleStringAfterModulo = QString::number(fmod(steeringAngle, 360), 'f', 0);
+  std::ostringstream steering_angle_ss;
+  if (last_msg_ptr_) {
+    steering_angle_ss << std::fixed << std::setprecision(1) << steering_angle_ * 180.0 / M_PI
+                      << "°";
+  } else {
+    steering_angle_ss << "N/A";
+  }
+
+  QString steeringAngleString = QString::fromStdString(steering_angle_ss.str());
+  // if the string doesn't have a negative sign, add a space to the front of the string
+  // to align the text in both cases (negative and positive)
+  if (steeringAngleString[0] != '-') {
+    steeringAngleString = " " + steeringAngleString;
+  }
 
   // Draw the steering angle text
-  QFont steeringFont("Quicksand", 9, QFont::Bold);
+  QFont steeringFont("Quicksand", 8, QFont::Bold);
   painter.setFont(steeringFont);
   painter.setPen(QColor(0, 0, 0, 255));
-  QRect steeringRect(
-    wheelCenterX - wheelImage.width() / 2, wheelCenterY - wheelImage.height() / 2,
-    wheelImage.width(), wheelImage.height());
-  painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleStringAfterModulo + "°");
+  // QRect steeringRect(
+  //   wheelCenterX - wheelImage.width() / 2 + 1, wheelCenterY - wheelImage.height() / 2,
+  //   wheelImage.width(), wheelImage.height());
+  // painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleString);
+
+  // Measure the text
+  QFontMetrics fontMetrics(steeringFont);
+  QRect textRect = fontMetrics.boundingRect(steeringAngleString);
+
+  // Center the text
+  int textX = wheelCenterX - textRect.width() / 2;
+  int textY = wheelCenterY - textRect.height() / 2;
+
+  QRect steeringRect(textX, textY, textRect.width(), textRect.height());
+  painter.drawText(steeringRect, Qt::AlignCenter, steeringAngleString);
 }
 
 QImage SteeringWheelDisplay::coloredImage(const QImage & source, const QColor & color)

Diff for: common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp

@@ -17,15 +17,17 @@
 
 #include "autoware/control_evaluator/metrics/deviation_metrics.hpp"
 
+#include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
-#include <array>
 #include <deque>
-#include <memory>
+#include <optional>
 #include <string>
 #include <utility>
 #include <vector>
@@ -39,6 +41,9 @@ using diagnostic_msgs::msg::DiagnosticStatus;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using nav_msgs::msg::Odometry;
+using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin;
+using autoware_planning_msgs::msg::LaneletRoute;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
 
 /**
  * @brief Node for control evaluation
@@ -62,6 +67,9 @@ class controlEvaluatorNode : public rclcpp::Node
     const DiagnosticArray & diag, const std::string & function_name);
 
   DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag);
+  DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const;
+  DiagnosticStatus generateKinematicStateDiagnosticStatus(
+    const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped);
 
   void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg);
   void onTimer();
@@ -74,11 +82,20 @@ class controlEvaluatorNode : public rclcpp::Node
 
   autoware::universe_utils::InterProcessPollingSubscriber<Odometry> odometry_sub_{
     this, "~/input/odometry"};
+  autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
+    this, "/localization/acceleration"};
   autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
     this, "~/input/trajectory"};
+  autoware::universe_utils::InterProcessPollingSubscriber<LaneletRoute> route_subscriber_{
+    this, "~/input/route", rclcpp::QoS{1}.transient_local()};
+  autoware::universe_utils::InterProcessPollingSubscriber<LaneletMapBin> vector_map_subscriber_{
+    this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()};
 
   rclcpp::Publisher<DiagnosticArray>::SharedPtr metrics_pub_;
 
+  // update Route Handler
+  void getRouteData();
+
   // Calculator
   // Metrics
   std::deque<rclcpp::Time> stamps_;
@@ -87,7 +104,9 @@ class controlEvaluatorNode : public rclcpp::Node
   std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
   const std::vector<std::string> target_functions_ = {"autonomous_emergency_braking"};
 
+  autoware::route_handler::RouteHandler route_handler_;
   rclcpp::TimerBase::SharedPtr timer_;
+  std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
 };
 }  // namespace control_diagnostics
 

Diff for: evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp

@@ -1,15 +1,21 @@
 <launch>
   <arg name="input/diagnostics" default="/diagnostics"/>
   <arg name="input/odometry" default="/localization/kinematic_state"/>
+  <arg name="input/acceleration" default="/localization/acceleration"/>
   <arg name="input/trajectory" default="/planning/scenario_planning/trajectory"/>
+  <arg name="map_topic_name" default="/map/vector_map"/>
+  <arg name="route_topic_name" default="/planning/mission_planning/route"/>
   <!-- control evaluator -->
   <group>
     <node name="control_evaluator" exec="control_evaluator" pkg="autoware_control_evaluator">
       <param from="$(find-pkg-share autoware_control_evaluator)/param/control_evaluator.defaults.yaml"/>
       <remap from="~/input/diagnostics" to="$(var input/diagnostics)"/>
       <remap from="~/input/odometry" to="$(var input/odometry)"/>
+      <remap from="~/input/acceleration" to="$(var input/acceleration)"/>
       <remap from="~/input/trajectory" to="$(var input/trajectory)"/>
       <remap from="~/metrics" to="/diagnostic/control_evaluator/metrics"/>
+      <remap from="~/input/vector_map" to="$(var map_topic_name)"/>
+      <remap from="~/input/route" to="$(var route_topic_name)"/>
     </node>
   </group>
 </launch>

Diff for: evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml

@@ -16,6 +16,7 @@
 
   <depend>autoware_motion_utils</depend>
   <depend>autoware_planning_msgs</depend>
+  <depend>autoware_route_handler</depend>
   <depend>autoware_universe_utils</depend>
   <depend>diagnostic_msgs</depend>
   <depend>pluginlib</depend>