Merge branch 'main' into refactor-node-config-shape_estimation

.cspell-partial.json

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

.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/goal_distance_calculator/config/goal_distance_calculator.param.yaml

@@ -1,3 +1,4 @@
 /**:
   ros__parameters:
     update_rate: 10.0
+    oneshot: false

common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml

@@ -1,8 +1,6 @@
 <launch>
-  <arg name="oneshot" default="false"/>
   <arg name="config_file" default="$(find-pkg-share goal_distance_calculator)/config/goal_distance_calculator.param.yaml"/>
   <node pkg="goal_distance_calculator" exec="goal_distance_calculator_node" name="goal_distance_calculator" output="screen">
     <param from="$(var config_file)"/>
-    <param name="oneshot" value="$(var oneshot)"/>
   </node>
 </launch>

common/goal_distance_calculator/schema/goal_distance_calculator.schema.json

@@ -0,0 +1,36 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for Goal Distance Calculator Node",
+  "type": "object",
+  "definitions": {
+    "goal_distance_calculator": {
+      "type": "object",
+      "properties": {
+        "update_rate": {
+          "type": "number",
+          "default": "10.0",
+          "exclusiveMinimum": 0,
+          "description": "Timer callback period. [Hz]"
+        },
+        "oneshot": {
+          "type": "boolean",
+          "default": "false",
+          "description": "Publish deviations just once or repeatedly."
+        }
+      },
+      "required": ["update_rate", "oneshot"]
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/goal_distance_calculator"
+        }
+      },
+      "required": ["ros__parameters"]
+    }
+  },
+  "required": ["/**"]
+}

common/goal_distance_calculator/src/goal_distance_calculator_node.cpp

@@ -40,8 +40,8 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions
   durable_qos.transient_local();
 
   // Node Parameter
-  node_param_.update_rate = declare_parameter("update_rate", 10.0);
-  node_param_.oneshot = declare_parameter("oneshot", true);
+  node_param_.update_rate = declare_parameter<double>("update_rate");
+  node_param_.oneshot = declare_parameter<bool>("oneshot");
 
   // Core
   goal_distance_calculator_ = std::make_unique<GoalDistanceCalculator>();

common/interpolation/package.xml

@@ -6,7 +6,6 @@
   <description>The spline interpolation package</description>
   <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
-  <maintainer email="yutaka.shimizu@tier4.jp">Yutaka Shimizu</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

common/kalman_filter/CMakeLists.txt

@@ -19,4 +19,11 @@ ament_auto_add_library(kalman_filter SHARED
   include/kalman_filter/time_delay_kalman_filter.hpp
 )
 
+if(BUILD_TESTING)
+  file(GLOB_RECURSE test_files test/*.cpp)
+  ament_add_ros_isolated_gtest(test_kalman_filter ${test_files})
+
+  target_link_libraries(test_kalman_filter kalman_filter)
+endif()
+
 ament_auto_package()

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/package.xml

@@ -18,7 +18,9 @@
   <build_depend>eigen3_cmake_module</build_depend>
 
   <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>