Merge branch 'main' into refactor/radar_object_clustering_params_to_a…

…utoware_launch

.cspell-partial.json

@@ -5,5 +5,5 @@
     "perception/bytetrack/lib/**"
   ],
   "ignoreRegExpList": [],
-  "words": []
+  "words": ["dltype", "tvmgen", "quantizer", "imageio", "mimsave"]
 }

.github/CODEOWNERS

@@ -56,7 +56,7 @@ common/traffic_light_recognition_marker_publisher/** [email protected]
 common/traffic_light_utils/** [email protected] [email protected]
 common/trtexec_vendor/** [email protected] [email protected]
 common/tvm_utility/** [email protected] [email protected]
-control/autonomous_emergency_braking/** [email protected] [email protected]
+control/autonomous_emergency_braking/** [email protected] [email protected] [email protected]
 control/control_performance_analysis/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 control/control_validator/** [email protected] [email protected] [email protected]
 control/external_cmd_selector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -66,6 +66,7 @@ control/mpc_lateral_controller/** [email protected] takayuki.murooka@tier
 control/obstacle_collision_checker/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 control/operation_mode_transition_manager/** [email protected] [email protected]
 control/pid_longitudinal_controller/** [email protected] [email protected]
+control/predicted_path_checker/** [email protected]
 control/pure_pursuit/** [email protected]
 control/shift_decider/** [email protected]
 control/trajectory_follower_base/** [email protected] [email protected]
@@ -87,13 +88,15 @@ launch/tier4_system_launch/** [email protected] [email protected]
 launch/tier4_vehicle_launch/** [email protected]
 localization/ekf_localizer/** [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/gyro_odometer/** [email protected] [email protected]
-localization/landmark_based_localizer/ar_tag_based_localizer/** [email protected] [email protected]
-localization/landmark_based_localizer/landmark_tf_caster/** [email protected] [email protected]
+localization/landmark_based_localizer/ar_tag_based_localizer/** [email protected] [email protected] shintaro.sakoda@tier4.jp yamato.ando@tier4.jp
+localization/landmark_based_localizer/landmark_parser/** [email protected] [email protected] shintaro.sakoda@tier4.jp yamato.ando@tier4.jp
 localization/localization_error_monitor/** [email protected] [email protected] [email protected]
+localization/localization_util/** [email protected] [email protected] [email protected] [email protected]
 localization/ndt_scan_matcher/** [email protected] [email protected] [email protected] [email protected]
 localization/pose2twist/** [email protected] [email protected]
 localization/pose_initializer/** [email protected] [email protected] [email protected]
 localization/stop_filter/** [email protected] [email protected] [email protected]
+localization/tree_structured_parzen_estimator/** [email protected] [email protected] [email protected] [email protected]
 localization/twist2accel/** [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_common/** [email protected] [email protected]
 localization/yabloc/yabloc_image_processing/** [email protected] [email protected]
@@ -140,6 +143,7 @@ perception/simple_object_merger/** [email protected] shunsuke.miura@tier4.
 perception/tensorrt_classifier/** [email protected]
 perception/tensorrt_yolo/** [email protected]
 perception/tensorrt_yolox/** [email protected] [email protected] [email protected]
+perception/tracking_object_merger/** [email protected] [email protected]
 perception/traffic_light_arbiter/** [email protected] [email protected]
 perception/traffic_light_classifier/** [email protected] [email protected]
 perception/traffic_light_fine_detector/** [email protected] [email protected]
@@ -150,7 +154,7 @@ perception/traffic_light_ssd_fine_detector/** [email protected]
 perception/traffic_light_visualization/** [email protected]
 planning/behavior_path_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_blind_spot_module/** [email protected] [email protected] [email protected]
-planning/behavior_velocity_crosswalk_module/** [email protected] [email protected] [email protected] [email protected]
+planning/behavior_velocity_crosswalk_module/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_detection_area_module/** [email protected] [email protected] [email protected]
 planning/behavior_velocity_intersection_module/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_no_drivable_lane_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -177,11 +181,10 @@ planning/obstacle_cruise_planner/** [email protected] [email protected]
 planning/obstacle_stop_planner/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/obstacle_velocity_limiter/** [email protected]
 planning/path_smoother/** [email protected] [email protected]
-planning/planning_debug_tools/** [email protected] [email protected]
+planning/planning_debug_tools/** [email protected] [email protected] [email protected]
 planning/planning_test_utils/** [email protected] [email protected]
 planning/planning_validator/** [email protected] [email protected]
 planning/route_handler/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-planning/rtc_auto_mode_manager/** [email protected] [email protected]
 planning/rtc_interface/** [email protected] [email protected]
 planning/rtc_replayer/** [email protected] [email protected]
 planning/sampling_based_planner/bezier_sampler/** [email protected]
@@ -215,11 +218,11 @@ system/default_ad_api_helpers/ad_api_visualizers/** [email protected] kahhoo
 system/default_ad_api_helpers/automatic_pose_initializer/** [email protected] [email protected] [email protected] [email protected] [email protected]
 system/dummy_diag_publisher/** [email protected] [email protected]
 system/dummy_infrastructure/** [email protected]
+system/duplicated_node_checker/** [email protected] [email protected]
 system/emergency_handler/** [email protected]
 system/mrm_comfortable_stop_operator/** [email protected]
 system/mrm_emergency_stop_operator/** [email protected]
 system/system_diagnostic_graph/** [email protected]
-system/system_diagnostic_monitor/** [email protected]
 system/system_error_monitor/** [email protected]
 system/system_monitor/** [email protected] [email protected]
 system/topic_state_monitor/** [email protected]

.github/dependabot.yaml

@@ -6,5 +6,5 @@ updates:
       interval: daily
     open-pull-requests-limit: 1
     labels:
-      - bot
-      - github-actions
+      - tag:bot
+      - type:github-actions

.github/labeler.yaml

@@ -1,41 +1,41 @@
-ci:
+"type:ci":
   - .github/**/*
   - "*.json"
   - "*.yaml"
   - "*.cfg"
   - .clang-format
   - .gitignore
   - .prettierignore
-documentation:
+"type:documentation":
   - docs/**/*
   - "**/*.md"
   - "**/*.rst"
   - "**/*.jpg"
   - "**/*.png"
   - "**/*.svg"
-common:
+"component:common":
   - common/**/*
-control:
+"component:control":
   - control/**/*
-evaluator:
+"component:evaluator":
   - evaluator/**/*
-launch:
+"component:launch":
   - launch/**/*
-localization:
+"component:localization":
   - localization/**/*
-map:
+"component:map":
   - map/**/*
-perception:
+"component:perception":
   - perception/**/*
-planning:
+"component:planning":
   - planning/**/*
-sensing:
+"component:sensing":
   - sensing/**/*
-simulator:
+"component:simulation":
   - simulator/**/*
-system:
+"component:system":
   - system/**/*
-tools:
+"component:tools":
   - tools/**/*
-vehicle:
+"component:vehicle":
   - vehicle/**/*

.github/stale.yml

@@ -4,7 +4,7 @@
 daysUntilClose: false
 
 # Label to use when marking as stale
-staleLabel: stale
+staleLabel: status:stale
 
 # Comment to post when marking as stale
 markComment: >

.github/sync-files.yaml

@@ -84,4 +84,8 @@
         sd "/edit/main/docs/" "/edit/main/" {source}
         sd "docs_dir: .*" "docs_dir: ." {source}
         sd "assets/(\w+)" "docs/assets/\$1" {source}
+        sd -- \
+        "  - macros" \
+        "  - macros:
+              module_name: mkdocs_macros" {source}
     - source: docs/assets/js/mathjax.js

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

@@ -11,7 +11,7 @@ jobs:
   prevent-no-label-execution:
     uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1
     with:
-      label: run-build-and-test-differential
+      label: tag:run-build-and-test-differential
 
   build-and-test-differential:
     needs: prevent-no-label-execution

.github/workflows/deploy-docs.yaml

@@ -22,7 +22,7 @@ jobs:
   prevent-no-label-execution:
     uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1
     with:
-      label: deploy-docs
+      label: tag:deploy-docs
 
   deploy-docs:
     needs: prevent-no-label-execution

.github/workflows/openai-pr-reviewer.yaml

@@ -21,7 +21,7 @@ jobs:
   prevent-no-label-execution:
     uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1
     with:
-      label: openai-pr-reviewer
+      label: tag:openai-pr-reviewer
   review:
     needs: prevent-no-label-execution
     if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }}

.github/workflows/sync-files.yaml

@@ -28,6 +28,6 @@ jobs:
         with:
           token: ${{ steps.generate-token.outputs.token }}
           pr-labels: |
-            bot
-            sync-files
+            tag:bot
+            tag:sync-files
           auto-merge-method: squash

.github/workflows/update-codeowners-from-packages.yaml

@@ -28,6 +28,6 @@ jobs:
         with:
           token: ${{ steps.generate-token.outputs.token }}
           pr-labels: |
-            bot
-            update-codeowners-from-packages
+            tag:bot
+            tag:update-codeowners-from-packages
           auto-merge-method: squash

common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp

@@ -24,9 +24,11 @@
 
 #include <condition_variable>
 #include <list>
+#include <queue>
 #include <set>
 #include <string>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 namespace autoware
@@ -45,10 +47,31 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
   using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects;
 
   PredictedObjectsDisplay();
+  ~PredictedObjectsDisplay()
+  {
+    {
+      std::unique_lock<std::mutex> lock(queue_mutex);
+      should_terminate = true;
+    }
+    condition.notify_all();
+    for (std::thread & active_thread : threads) {
+      active_thread.join();
+    }
+    threads.clear();
+  }
 
 private:
   void processMessage(PredictedObjects::ConstSharedPtr msg) override;
 
+  void queueJob(std::function<void()> job)
+  {
+    {
+      std::unique_lock<std::mutex> lock(queue_mutex);
+      jobs.push(std::move(job));
+    }
+    condition.notify_one();
+  }
+
   boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg)
   {
     const std::string uuid_str = uuid_to_string(uuid_msg);
@@ -100,6 +123,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
     PredictedObjects::ConstSharedPtr msg);
   void workerThread();
 
+  void messageProcessorThreadJob();
+
   void update(float wall_dt, float ros_dt) override;
 
   std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map;
@@ -108,6 +133,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
   int32_t marker_id = 0;
   const int32_t PATH_ID_CONSTANT = 1e3;
 
+  // max_num_threads: number of threads created in the thread pool, hard-coded to be 1;
+  int max_num_threads;
+
+  bool should_terminate{false};
+  std::mutex queue_mutex;
+  std::vector<std::thread> threads;
+  std::queue<std::function<void()>> jobs;
+
   PredictedObjects::ConstSharedPtr msg;
   bool consumed{false};
   std::mutex mutex;

common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp

@@ -25,27 +25,44 @@ namespace object_detection
 {
 PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks")
 {
-  std::thread worker(&PredictedObjectsDisplay::workerThread, this);
-  worker.detach();
+  max_num_threads = 1;  // hard code the number of threads to be created
+
+  for (int ii = 0; ii < max_num_threads; ++ii) {
+    threads.emplace_back(std::thread(&PredictedObjectsDisplay::workerThread, this));
+  }
 }
 
 void PredictedObjectsDisplay::workerThread()
-{
+{  // A standard working thread that waiting for jobs
   while (true) {
-    std::unique_lock<std::mutex> lock(mutex);
-    condition.wait(lock, [this] { return this->msg; });
+    std::function<void()> job;
+    {
+      std::unique_lock<std::mutex> lock(queue_mutex);
+      condition.wait(lock, [this] { return !jobs.empty() || should_terminate; });
+      if (should_terminate) {
+        return;
+      }
+      job = jobs.front();
+      jobs.pop();
+    }
+    job();
+  }
+}
 
-    auto tmp_msg = this->msg;
-    this->msg.reset();
+void PredictedObjectsDisplay::messageProcessorThreadJob()
+{
+  // Receiving
+  std::unique_lock<std::mutex> lock(mutex);
+  auto tmp_msg = this->msg;
+  this->msg.reset();
+  lock.unlock();
 
-    lock.unlock();
+  auto tmp_markers = createMarkers(tmp_msg);
 
-    auto tmp_markers = createMarkers(tmp_msg);
-    lock.lock();
-    markers = tmp_markers;
+  lock.lock();
+  markers = tmp_markers;
 
-    consumed = true;
-  }
+  consumed = true;
 }
 
 std::vector<visualization_msgs::msg::Marker::SharedPtr> PredictedObjectsDisplay::createMarkers(
@@ -188,7 +205,7 @@ void PredictedObjectsDisplay::processMessage(PredictedObjects::ConstSharedPtr ms
   std::unique_lock<std::mutex> lock(mutex);
 
   this->msg = msg;
-  condition.notify_one();
+  queueJob(std::bind(&PredictedObjectsDisplay::messageProcessorThreadJob, this));
 }
 
 void PredictedObjectsDisplay::update(float wall_dt, float ros_dt)

common/interpolation/package.xml

@@ -6,7 +6,6 @@
   <description>The spline interpolation package</description>
   <maintainer email="[email protected]">Fumiya Watanabe</maintainer>
   <maintainer email="[email protected]">Takayuki Murooka</maintainer>
-  <maintainer email="[email protected]">Yutaka Shimizu</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

common/kalman_filter/include/kalman_filter/kalman_filter.hpp

@@ -109,20 +109,20 @@ class KalmanFilter
    * @brief get current kalman filter state
    * @param x kalman filter state
    */
-  void getX(Eigen::MatrixXd & x);
+  void getX(Eigen::MatrixXd & x) const;
 
   /**
    * @brief get current kalman filter covariance
    * @param P kalman filter covariance
    */
-  void getP(Eigen::MatrixXd & P);
+  void getP(Eigen::MatrixXd & P) const;
 
   /**
    * @brief get component of current kalman filter state
    * @param i index of kalman filter state
    * @return value of i's component of the kalman filter state x[i]
    */
-  double getXelement(unsigned int i);
+  double getXelement(unsigned int i) const;
 
   /**
    * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix.

common/kalman_filter/src/kalman_filter.cpp

@@ -77,15 +77,15 @@ void KalmanFilter::setR(const Eigen::MatrixXd & R)
 {
   R_ = R;
 }
-void KalmanFilter::getX(Eigen::MatrixXd & x)
+void KalmanFilter::getX(Eigen::MatrixXd & x) const
 {
   x = x_;
 }
-void KalmanFilter::getP(Eigen::MatrixXd & P)
+void KalmanFilter::getP(Eigen::MatrixXd & P) const
 {
   P = P_;
 }
-double KalmanFilter::getXelement(unsigned int i)
+double KalmanFilter::getXelement(unsigned int i) const
 {
   return x_(i);
 }