Merge pull request #1042 from tier4/beta-to-tier4-main-sync

chore: sync beta branch beta/v0.17.0 with tier4/main

.github/labeler.yaml

@@ -31,7 +31,7 @@
   - planning/**/*
 "component:sensing":
   - sensing/**/*
-"component:simulator":
+"component:simulation":
   - simulator/**/*
 "component:system":
   - system/**/*

.github/sync-files.yaml

@@ -12,8 +12,6 @@
     - source: .github/PULL_REQUEST_TEMPLATE/standard-change.md
     - source: .github/dependabot.yaml
     - source: .github/stale.yml
-      pre-commands: |
-        sd "staleLabel: stale" "staleLabel: status:stale" {source}
     - source: .github/workflows/cancel-previous-workflows.yaml
     - source: .github/workflows/github-release.yaml
     - source: .github/workflows/pre-commit.yaml

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/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -168,6 +168,7 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0)
   }
 
   T dst;
+  dst.reserve(points.size());
 
   for (size_t i = 0; i <= start_idx; ++i) {
     dst.push_back(points.at(i));
@@ -1251,7 +1252,7 @@ calcLongitudinalOffsetPose<std::vector<autoware_auto_planning_msgs::msg::Traject
  * @param offset length of offset from source point
  * @param set_orientation_from_position_direction set orientation by spherical interpolation if
  * false
- * @return offset pase
+ * @return offset pose
  */
 template <class T>
 boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(

common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp

@@ -0,0 +1,77 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "motion_utils/trajectory/trajectory.hpp"
+
+#include <gtest/gtest.h>
+#include <gtest/internal/gtest-port.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <random>
+
+namespace
+{
+using autoware_auto_planning_msgs::msg::Trajectory;
+using tier4_autoware_utils::createPoint;
+using tier4_autoware_utils::createQuaternionFromRPY;
+
+constexpr double epsilon = 1e-6;
+
+geometry_msgs::msg::Pose createPose(
+  double x, double y, double z, double roll, double pitch, double yaw)
+{
+  geometry_msgs::msg::Pose p;
+  p.position = createPoint(x, y, z);
+  p.orientation = createQuaternionFromRPY(roll, pitch, yaw);
+  return p;
+}
+
+template <class T>
+T generateTestTrajectory(
+  const size_t num_points, const double point_interval, const double vel = 0.0,
+  const double init_theta = 0.0, const double delta_theta = 0.0)
+{
+  using Point = typename T::_points_type::value_type;
+
+  T traj;
+  for (size_t i = 0; i < num_points; ++i) {
+    const double theta = init_theta + i * delta_theta;
+    const double x = i * point_interval * std::cos(theta);
+    const double y = i * point_interval * std::sin(theta);
+
+    Point p;
+    p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta);
+    p.longitudinal_velocity_mps = vel;
+    traj.points.push_back(p);
+  }
+
+  return traj;
+}
+}  // namespace
+
+TEST(trajectory_benchmark, DISABLED_calcLateralOffset)
+{
+  std::random_device r;
+  std::default_random_engine e1(r());
+  std::uniform_real_distribution<double> uniform_dist(0.0, 1000.0);
+
+  using motion_utils::calcLateralOffset;
+
+  const auto traj = generateTestTrajectory<Trajectory>(1000, 1.0, 0.0, 0.0, 0.1);
+  constexpr auto nb_iteration = 10000;
+  for (auto i = 0; i < nb_iteration; ++i) {
+    const auto point = createPoint(uniform_dist(e1), uniform_dist(e1), 0.0);
+    calcLateralOffset(traj.points, point);
+  }
+}

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

@@ -29,6 +29,7 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <boost/geometry/index/rtree.hpp>
 #include <boost/optional.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
@@ -46,7 +47,9 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using autoware_planning_msgs::msg::LaneletRoute;
 using tier4_autoware_utils::LinearRing2d;
 using tier4_autoware_utils::PoseDeviation;
+using tier4_autoware_utils::Segment2d;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
+typedef boost::geometry::index::rtree<Segment2d, boost::geometry::index::rstar<16>> SegmentRtree;
 
 struct Param
 {
@@ -137,13 +140,15 @@ class LaneDepartureChecker
     const lanelet::ConstLanelets & candidate_lanelets,
     const std::vector<LinearRing2d> & vehicle_footprints);
 
+  double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const;
+
+  static SegmentRtree extractUncrossableBoundaries(
+    const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point,
+    const double max_search_length, const std::vector<std::string> & boundary_types_to_detect);
+
   static bool willCrossBoundary(
-    const lanelet::ConstLanelets & candidate_lanelets,
     const std::vector<LinearRing2d> & vehicle_footprints,
-    const std::vector<std::string> & boundary_types_to_detects);
-
-  static bool isCrossingRoadBorder(
-    const lanelet::BasicLineString2d & road_border, const std::vector<LinearRing2d> & footprints);
+    const SegmentRtree & uncrossable_segments);
 };
 }  // namespace lane_departure_checker