Merge branch 'main' into feature/simulator_py_model

Author: atomyks

.github/CODEOWNERS

@@ -77,7 +77,7 @@ evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4
 evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp
 evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
 evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp
-evaluator/tier4_metrics_rviz_plugin/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp
+evaluator/tier4_metrics_rviz_plugin/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp
 launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp
 launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
 launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
@@ -121,7 +121,7 @@ perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp
 perception/detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
 perception/detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp
-perception/euclidean_cluster/** yukihiro.saito@tier4.jp
+perception/euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp
 perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
 perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
 perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp
@@ -212,7 +212,7 @@ sensing/image_diagnostics/** dai.nguyen@tier4.jp
 sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp
 sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
 sensing/livox/livox_tag_filter/** ryohsuke.mitsudome@tier4.jp
-sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
+sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
 sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
 sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
 sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp

.github/workflows/build-and-test-self-hosted.yaml .github/workflows/build-and-test-arm64.yaml

@@ -1,12 +1,12 @@
-name: build-and-test-self-hosted
+name: build-and-test-arm64
 
 on:
   schedule:
     - cron: 0 0 * * *
   workflow_dispatch:
 
 jobs:
-  build-and-test-self-hosted:
+  build-and-test-arm64:
     runs-on: [self-hosted, linux, ARM64]
     container: ${{ matrix.container }}${{ matrix.container-suffix }}
     strategy:
@@ -19,12 +19,15 @@ jobs:
           - -cuda
         include:
           - rosdistro: humble
-            container: ghcr.io/autowarefoundation/autoware-universe:humble-latest
+            container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt
             build-depends-repos: build_depends.repos
     steps:
       - name: Check out repository
         uses: actions/checkout@v3
 
+      - name: Show disk space before the tasks
+        run: df -h
+
       - name: Remove exec_depend
         uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
 
@@ -47,3 +50,6 @@ jobs:
           rosdistro: ${{ matrix.rosdistro }}
           target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
           build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Show disk space after the tasks
+        run: df -h

.github/workflows/build-and-test-differential-self-hosted.yaml .github/workflows/build-and-test-differential-arm64.yaml

@@ -1,4 +1,4 @@
-name: build-and-test-differential-self-hosted
+name: build-and-test-differential-arm64
 
 on:
   pull_request:
@@ -12,9 +12,9 @@ jobs:
   prevent-no-label-execution:
     uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1
     with:
-      label: ARM64
+      label: type:arm64
 
-  build-and-test-differential-self-hosted:
+  build-and-test-differential-arm64:
     needs: prevent-no-label-execution
     if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }}
     runs-on: [self-hosted, linux, ARM64]
@@ -29,14 +29,17 @@ jobs:
           - -cuda
         include:
           - rosdistro: humble
-            container: ghcr.io/autowarefoundation/autoware-universe:humble-latest
+            container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt
             build-depends-repos: build_depends.repos
     steps:
       - name: Check out repository
         uses: actions/checkout@v3
         with:
           fetch-depth: 0
 
+      - name: Show disk space before the tasks
+        run: df -h
+
       - name: Remove exec_depend
         uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
 
@@ -59,3 +62,6 @@ jobs:
           rosdistro: ${{ matrix.rosdistro }}
           target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
           build-depends-repos: ${{ matrix.build-depends-repos }}
+
+      - name: Show disk space after the tasks
+        run: df -h

.github/workflows/build-and-test-differential.yaml

@@ -16,26 +16,27 @@ jobs:
   build-and-test-differential:
     needs: prevent-no-label-execution
     if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }}
-    runs-on: [self-hosted, linux, X64]
+    runs-on: ubuntu-latest
     container: ${{ matrix.container }}${{ matrix.container-suffix }}
     strategy:
       fail-fast: false
       matrix:
         rosdistro:
           - humble
         container-suffix:
+          - ""
           - -cuda
         include:
           - rosdistro: humble
-            container: ghcr.io/autowarefoundation/autoware-universe:humble-latest
+            container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt
             build-depends-repos: build_depends.repos
     steps:
       - name: Check out repository
         uses: actions/checkout@v3
         with:
           fetch-depth: 0
 
-      - name: Check disk space before build
+      - name: Show disk space before the tasks
         run: df -h
 
       - name: Remove exec_depend
@@ -71,5 +72,5 @@ jobs:
           verbose: true
           flags: differential
 
-      - name: Check disk space after build
+      - name: Show disk space after the tasks
         run: df -h

.github/workflows/build-and-test.yaml

@@ -9,7 +9,7 @@ on:
 jobs:
   build-and-test:
     if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }}
-    runs-on: [self-hosted, linux, X64]
+    runs-on: ubuntu-latest
     container: ${{ matrix.container }}${{ matrix.container-suffix }}
     strategy:
       fail-fast: false
@@ -21,12 +21,15 @@ jobs:
           - -cuda
         include:
           - rosdistro: humble
-            container: ghcr.io/autowarefoundation/autoware-universe:humble-latest
+            container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt
             build-depends-repos: build_depends.repos
     steps:
       - name: Check out repository
         uses: actions/checkout@v3
 
+      - name: Show disk space before the tasks
+        run: df -h
+
       - name: Remove exec_depend
         uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
 
@@ -59,3 +62,6 @@ jobs:
           fail_ci_if_error: false
           verbose: true
           flags: total
+
+      - name: Show disk space after the tasks
+        run: df -h

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;
 

control/mpc_lateral_controller/src/mpc_lateral_controller.cpp

@@ -243,7 +243,11 @@ trajectory_follower::LateralOutput MpcLateralController::run(
   Trajectory predicted_traj;
   Float32MultiArrayStamped debug_values;
 
-  if (!m_is_ctrl_cmd_prev_initialized) {
+  const bool is_under_control = input_data.current_operation_mode.is_autoware_control_enabled &&
+                                input_data.current_operation_mode.mode ==
+                                  autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS;
+
+  if (!m_is_ctrl_cmd_prev_initialized || !is_under_control) {
     m_ctrl_cmd_prev = getInitialControlCommand();
     m_is_ctrl_cmd_prev_initialized = true;
   }

evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp

@@ -19,6 +19,7 @@
 #include "perception_online_evaluator/metrics/metric.hpp"
 #include "perception_online_evaluator/parameters.hpp"
 #include "perception_online_evaluator/stat.hpp"
+#include "perception_online_evaluator/utils/objects_filtering.hpp"
 
 #include <rclcpp/time.hpp>
 
@@ -118,9 +119,9 @@ class MetricsCalculator
   void deleteOldObjects(const rclcpp::Time stamp);
 
   // Calculate metrics
-  MetricStatMap calcLateralDeviationMetrics(const PredictedObjects & objects) const;
-  MetricStatMap calcYawDeviationMetrics(const PredictedObjects & objects) const;
-  MetricStatMap calcPredictedPathDeviationMetrics(const PredictedObjects & objects) const;
+  MetricStatMap calcLateralDeviationMetrics(const ClassObjectsMap & class_objects_map) const;
+  MetricStatMap calcYawDeviationMetrics(const ClassObjectsMap & class_objects_map) const;
+  MetricStatMap calcPredictedPathDeviationMetrics(const ClassObjectsMap & class_objects_map) const;
   Stat<double> calcPredictedPathDeviationMetrics(
     const PredictedObjects & objects, const double time_horizon) const;
 

evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp

@@ -37,6 +37,8 @@ using autoware_auto_perception_msgs::msg::ObjectClassification;
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 
+using ClassObjectsMap = std::unordered_map<uint8_t, PredictedObjects>;
+
 std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
 
 /**
@@ -105,6 +107,14 @@ void filterObjects(PredictedObjects & objects, Func filter)
   filterObjects(objects, removed_objects, filter);
 }
 
+/**
+ * @brief Separates the provided objects based on their classification.
+ *
+ * @param objects The predicted objects to be separated.
+ * @return A map of objects separated by their classification.
+ */
+ClassObjectsMap separateObjectsByClass(const PredictedObjects & objects);
+
 /**
  * @brief Filters the provided objects based on their classification.
  *

evaluator/perception_online_evaluator/package.xml

@@ -24,6 +24,7 @@
   <depend>libgoogle-glog-dev</depend>
   <depend>motion_utils</depend>
   <depend>nav_msgs</depend>
+  <depend>object_recognition_utils</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>