Merge branch 'main' into feat/fix_iou_functions_roi_cluster_fusion

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));

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);
+  }
+}

launch/tier4_localization_launch/launch/pose_twist_estimator/eagleye/eagleye_rt.launch.xml

@@ -145,9 +145,13 @@
   </group>
 
   <group if="$(var use_eagleye_pose)">
-    <include file="$(find-pkg-share eagleye_geo_pose_fusion)/launch/geo_pose_fusion.launch.xml"/>
-    <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/eagleye/geo_pose_converter.launch.xml">
-      <arg name="output_pose_with_cov_name" value="$(var output_pose_with_cov_name)"/>
+    <arg name="geo_pose_topic_name" default="eagleye/geo_pose_with_covariance"/>
+    <include file="$(find-pkg-share eagleye_geo_pose_fusion)/launch/geo_pose_fusion.launch.xml">
+      <arg name="output_geo_pose" value="$(var geo_pose_topic_name)"/>
+    </include>
+    <include file="$(find-pkg-share geo_pose_projector)/launch/geo_pose_projector.launch.xml">
+      <arg name="input_geo_pose" value="$(var geo_pose_topic_name)"/>
+      <arg name="output_pose" value="$(var output_pose_with_cov_name)"/>
     </include>
   </group>
 </launch>

launch/tier4_localization_launch/package.xml

@@ -16,11 +16,11 @@
 
   <exec_depend>ar_tag_based_localizer</exec_depend>
   <exec_depend>automatic_pose_initializer</exec_depend>
-  <exec_depend>eagleye_geo_pose_converter</exec_depend>
   <exec_depend>eagleye_geo_pose_fusion</exec_depend>
   <exec_depend>eagleye_gnss_converter</exec_depend>
   <exec_depend>eagleye_rt</exec_depend>
   <exec_depend>ekf_localizer</exec_depend>
+  <exec_depend>geo_pose_projector</exec_depend>
   <exec_depend>gyro_odometer</exec_depend>
   <exec_depend>ndt_scan_matcher</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -195,6 +195,7 @@
   <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
+    <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   </group>
 
   <!-- CenterPoint -->

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml

@@ -220,6 +220,7 @@
   <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
+    <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   </group>
 
   <!-- CenterPoint -->

launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml

@@ -73,6 +73,7 @@
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
       <arg name="input/radar" value="$(var input/radar)"/>
       <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
+      <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
     </include>
   </group>
 
@@ -104,6 +105,7 @@
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
+      <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
     </include>
   </group>
 
@@ -133,6 +135,7 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+      <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml

@@ -61,6 +61,7 @@
   <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
+    <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   </group>
 
   <!-- CenterPoint -->

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -18,6 +18,7 @@
   <arg name="obstacle_segmentation_ground_segmentation_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
   <arg name="object_recognition_detection_obstacle_pointcloud_based_validator_param_path"/>
+  <arg name="object_recognition_detection_detection_by_tracker_param"/>
   <arg name="occupancy_grid_map_method"/>
   <arg name="occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater"/>
@@ -174,6 +175,7 @@
           <arg name="outlier_param_path" value="$(var object_recognition_detection_outlier_param_path)"/>
           <arg name="voxel_grid_based_euclidean_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_cluster_param_path)"/>
           <arg name="radar_lanelet_filtering_range_param" value="$(var object_recognition_detection_radar_lanelet_filtering_range_param)"/>
+          <arg name="detection_by_tracker_param_path" value="$(var object_recognition_detection_detection_by_tracker_param)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
           <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
           <arg name="use_object_filter" value="$(var use_object_filter)"/>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

@@ -127,8 +127,8 @@
           <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
           <remap from="~/output/trajectory" to="$(var interface_output_topic)"/>
           <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
-          <remap from="~/output/max_velocity" to="/planning/scenario_planning/max_velocity_candidates"/>
-          <remap from="~/output/velocity_limit_clear_command" to="/planning/scenario_planning/clear_velocity_limit"/>
+          <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
+          <remap from="~/output/clear_velocity_limit" to="/planning/scenario_planning/clear_velocity_limit"/>
           <!-- params -->
           <param from="$(var common_param_path)"/>
           <param from="$(var vehicle_param_file)"/>

localization/geo_pose_projector/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.14)
+project(geo_pose_projector)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(geo_pose_projector
+  src/geo_pose_projector_node.cpp
+  src/geo_pose_projector.cpp
+)
+ament_target_dependencies(geo_pose_projector)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

localization/geo_pose_projector/README.md

@@ -0,0 +1,28 @@
+# geo_pose_projector
+
+## Overview
+
+This node is a simple node that subscribes to the geo-referenced pose topic and publishes the pose in the map frame.
+
+## Subscribed Topics
+
+| Name                      | Type                                                 | Description         |
+| ------------------------- | ---------------------------------------------------- | ------------------- |
+| `input_geo_pose`          | `geographic_msgs::msg::GeoPoseWithCovarianceStamped` | geo-referenced pose |
+| `/map/map_projector_info` | `tier4_map_msgs::msg::MapProjectedObjectInfo`        | map projector info  |
+
+## Published Topics
+
+| Name          | Type                                            | Description                           |
+| ------------- | ----------------------------------------------- | ------------------------------------- |
+| `output_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | pose in map frame                     |
+| `/tf`         | `tf2_msgs::msg::TFMessage`                      | tf from parent link to the child link |
+
+## Parameters
+
+{{ json_to_markdown("localization/geo_pose_projector/schema/geo_pose_projector.schema.json") }}
+
+## Limitations
+
+The covariance conversion may be incorrect depending on the projection type you are using. The covariance of input topic is expressed in (Latitude, Longitude, Altitude) as a diagonal matrix.
+Currently, we assume that the x axis is the east direction and the y axis is the north direction. Thus, the conversion may be incorrect when this assumption breaks, especially when the covariance of latitude and longitude is different.