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

…n_node_update_azimuth_and_distance

.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

common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp

@@ -30,14 +30,6 @@ using std::placeholders::_1;
 namespace rviz_plugins
 {
 
-double lowpassFilter(
-  const double current_value, const double prev_value, double cutoff, const double dt)
-{
-  const double tau = 1.0 / (2.0 * M_PI * cutoff);
-  const double a = tau / (dt + tau);
-  return prev_value * a + (1.0 - a) * current_value;
-}
-
 ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent)
 {
   auto * state_layout = new QHBoxLayout;
@@ -115,25 +107,23 @@ void ManualController::update()
     ackermann.stamp = raw_node_->get_clock()->now();
     ackermann.lateral.steering_tire_angle = steering_angle_;
     ackermann.longitudinal.speed = cruise_velocity_;
-    if (current_acceleration_) {
-      /**
-       * @brief Calculate desired acceleration by simple BackSteppingControl
-       *  V = 0.5*(v-v_des)^2 >= 0
-       *  D[V] = (D[v] - a_des)*(v-v_des) <=0
-       *  a_des = k_const *(v - v_des) + a (k < 0 )
-       */
-      const double k = -0.5;
-      const double v = current_velocity_;
-      const double v_des = cruise_velocity_;
-      const double a = *current_acceleration_;
-      const double a_des = k * (v - v_des) + a;
-      ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0);
-    }
+    /**
+     * @brief Calculate desired acceleration by simple BackSteppingControl
+     *  V = 0.5*(v-v_des)^2 >= 0
+     *  D[V] = (D[v] - a_des)*(v-v_des) <=0
+     *  a_des = k_const *(v - v_des) + a (k < 0 )
+     */
+    const double k = -0.5;
+    const double v = current_velocity_;
+    const double v_des = cruise_velocity_;
+    const double a = current_acceleration_;
+    const double a_des = k * (v - v_des) + a;
+    ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0);
   }
   GearCommand gear_cmd;
   {
     const double eps = 0.001;
-    if (ackermann.longitudinal.speed > eps) {
+    if (ackermann.longitudinal.speed > eps && current_velocity_ > -eps) {
       gear_cmd.command = GearCommand::DRIVE;
     } else if (ackermann.longitudinal.speed < -eps && current_velocity_ < eps) {
       gear_cmd.command = GearCommand::REVERSE;
@@ -220,19 +210,11 @@ void ManualController::onEngageStatus(const Engage::ConstSharedPtr msg)
 void ManualController::onVelocity(const VelocityReport::ConstSharedPtr msg)
 {
   current_velocity_ = msg->longitudinal_velocity;
-  if (previous_velocity_) {
-    const double cutoff = 10.0;
-    const double dt = 1.0 / 10.0;
-    const double acc = (current_velocity_ - *previous_velocity_) / dt;
-    if (!current_acceleration_) {
-      current_acceleration_ = std::make_unique<double>(acc);
-    } else {
-      current_acceleration_ =
-        std::make_unique<double>(lowpassFilter(acc, *current_acceleration_, cutoff, dt));
-    }
-  }
-  previous_velocity_ = std::make_unique<double>(msg->longitudinal_velocity);
-  prev_stamp_ = rclcpp::Time(msg->header.stamp);
+}
+
+void ManualController::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg)
+{
+  current_acceleration_ = msg->accel.accel.linear.x;
 }
 
 void ManualController::onGear(const GearReport::ConstSharedPtr msg)

common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp

@@ -25,6 +25,7 @@
 #include <rviz_common/panel.hpp>
 
 #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/engage.hpp>
@@ -40,6 +41,7 @@ namespace rviz_plugins
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
 using autoware_auto_vehicle_msgs::msg::GearCommand;
 using autoware_auto_vehicle_msgs::msg::VelocityReport;
+using geometry_msgs::msg::AccelWithCovarianceStamped;
 using geometry_msgs::msg::Twist;
 using tier4_control_msgs::msg::GateMode;
 using EngageSrv = tier4_external_api_msgs::srv::Engage;
@@ -67,6 +69,7 @@ public Q_SLOTS:  // NOLINT for Qt
   void onPublishControlCommand();
   void onGateMode(const GateMode::ConstSharedPtr msg);
   void onVelocity(const VelocityReport::ConstSharedPtr msg);
+  void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg);
   void onEngageStatus(const Engage::ConstSharedPtr msg);
   void onGear(const GearReport::ConstSharedPtr msg);
   rclcpp::Node::SharedPtr raw_node_;
@@ -82,9 +85,7 @@ public Q_SLOTS:  // NOLINT for Qt
   double cruise_velocity_{0.0};
   double steering_angle_{0.0};
   double current_velocity_{0.0};
-  rclcpp::Time prev_stamp_;
-  std::unique_ptr<double> previous_velocity_;
-  std::unique_ptr<double> current_acceleration_;
+  double current_acceleration_{0.0};
 
   QLabel * gate_mode_label_ptr_;
   QLabel * gear_label_ptr_;

common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp

@@ -502,6 +502,11 @@ void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg)
         style_sheet = "background-color: #00FF00;";  // green
         break;
 
+      case MRMState::PULL_OVER:
+        text = "PULL_OVER";
+        style_sheet = "background-color: #FFFF00;";  // yellow
+        break;
+
       case MRMState::COMFORTABLE_STOP:
         text = "COMFORTABLE_STOP";
         style_sheet = "background-color: #FFFF00;";  // yellow

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>