chore: sync upstream

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

@@ -2,10 +2,21 @@ name: build-and-test-differential
 
 on:
   pull_request:
+    types:
+      - opened
+      - synchronize
+      - labeled
 
 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
+
   build-and-test-differential:
-    runs-on: ubuntu-latest
+    needs: prevent-no-label-execution
+    if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }}
+    runs-on: [self-hosted, linux, X64]
     container: ${{ matrix.container }}${{ matrix.container-suffix }}
     strategy:
       fail-fast: false
@@ -59,7 +70,7 @@ jobs:
           flags: differential
 
   clang-tidy-differential:
-    runs-on: ubuntu-latest
+    runs-on: [self-hosted, linux, X64]
     container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda
     needs: build-and-test-differential
     steps:

common/perception_utils/include/perception_utils/prime_synchronizer.hpp

@@ -76,7 +76,7 @@ class PrimeSynchronizer
       const typename PrimeMsgT::ConstSharedPtr, const typename SecondaryMsgT::ConstSharedPtr...)>
       callback,
     StampT max_delay_t = 0.2, StampT max_wait_t = 0.1)
-  : node_ptr_(node_ptr), callback_(callback), max_delay_t_(max_delay_t), max_wait_t_(max_wait_t)
+  : node_ptr_(node_ptr), callback_(callback), max_wait_t_(max_wait_t), max_delay_t_(max_delay_t)
   {
     assert((topics.size() == sizeof...(SecondaryMsgT) + 1) && "Incorrect topic number");
     assert(topics.size() == qos.size() && "topic size not equal to qos size!");

common/tensorrt_common/src/tensorrt_common.cpp

@@ -273,20 +273,20 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path)
   int total_params = 0;
   for (int i = 0; i < num; i++) {
     nvinfer1::ILayer * layer = network->getLayer(i);
-    auto ltype = layer->getType();
+    auto layer_type = layer->getType();
     std::string name = layer->getName();
     if (build_config_->profile_per_layer) {
       model_profiler_.setProfDict(layer);
     }
-    if (ltype == nvinfer1::LayerType::kCONSTANT) {
+    if (layer_type == nvinfer1::LayerType::kCONSTANT) {
       continue;
     }
     nvinfer1::ITensor * in = layer->getInput(0);
     nvinfer1::Dims dim_in = in->getDimensions();
     nvinfer1::ITensor * out = layer->getOutput(0);
     nvinfer1::Dims dim_out = out->getDimensions();
 
-    if (ltype == nvinfer1::LayerType::kCONVOLUTION) {
+    if (layer_type == nvinfer1::LayerType::kCONVOLUTION) {
       nvinfer1::IConvolutionLayer * conv = (nvinfer1::IConvolutionLayer *)layer;
       nvinfer1::Dims k_dims = conv->getKernelSizeNd();
       nvinfer1::Dims s_dims = conv->getStrideNd();
@@ -305,7 +305,7 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path)
       std::cout << " weights:" << num_weights;
       std::cout << " GFLOPs:" << gflops;
       std::cout << std::endl;
-    } else if (ltype == nvinfer1::LayerType::kPOOLING) {
+    } else if (layer_type == nvinfer1::LayerType::kPOOLING) {
       nvinfer1::IPoolingLayer * pool = (nvinfer1::IPoolingLayer *)layer;
       auto p_type = pool->getPoolingType();
       nvinfer1::Dims dim_stride = pool->getStrideNd();
@@ -325,7 +325,7 @@ void TrtCommon::printNetworkInfo(const std::string & onnx_file_path)
       std::cout << "pool " << dim_window.d[0] << "x" << dim_window.d[1] << "]";
       std::cout << " GFLOPs:" << gflops;
       std::cout << std::endl;
-    } else if (ltype == nvinfer1::LayerType::kRESIZE) {
+    } else if (layer_type == nvinfer1::LayerType::kRESIZE) {
       std::cout << "L" << i << " [resize]" << std::endl;
     }
   }
@@ -364,7 +364,7 @@ bool TrtCommon::buildEngineFromOnnx(
     if (num_available_dla > 0) {
       std::cout << "###" << num_available_dla << " DLAs are supported! ###" << std::endl;
     } else {
-      std::cout << "###Warninig : "
+      std::cout << "###Warning : "
                 << "No DLA is supported! ###" << std::endl;
     }
     config->setDefaultDeviceType(nvinfer1::DeviceType::kDLA);
@@ -400,7 +400,7 @@ bool TrtCommon::buildEngineFromOnnx(
     network->getInput(0)->setDynamicRange(0, 255.0);
     for (int i = 0; i < num; i++) {
       nvinfer1::ILayer * layer = network->getLayer(i);
-      auto ltype = layer->getType();
+      auto layer_type = layer->getType();
       std::string name = layer->getName();
       nvinfer1::ITensor * out = layer->getOutput(0);
       if (build_config_->clip_value > 0.0) {
@@ -409,7 +409,7 @@ bool TrtCommon::buildEngineFromOnnx(
         out->setDynamicRange(0.0, build_config_->clip_value);
       }
 
-      if (ltype == nvinfer1::LayerType::kCONVOLUTION) {
+      if (layer_type == nvinfer1::LayerType::kCONVOLUTION) {
         if (first) {
           layer->setPrecision(nvinfer1::DataType::kHALF);
           std::cout << "Set kHALF in " << name << std::endl;
@@ -424,14 +424,14 @@ bool TrtCommon::buildEngineFromOnnx(
           }
           for (int i = num - 1; i >= 0; i--) {
             nvinfer1::ILayer * layer = network->getLayer(i);
-            auto ltype = layer->getType();
+            auto layer_type = layer->getType();
             std::string name = layer->getName();
-            if (ltype == nvinfer1::LayerType::kCONVOLUTION) {
+            if (layer_type == nvinfer1::LayerType::kCONVOLUTION) {
               layer->setPrecision(nvinfer1::DataType::kHALF);
               std::cout << "Set kHALF in " << name << std::endl;
               break;
             }
-            if (ltype == nvinfer1::LayerType::kMATRIX_MULTIPLY) {
+            if (layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) {
               layer->setPrecision(nvinfer1::DataType::kHALF);
               std::cout << "Set kHALF in " << name << std::endl;
               break;

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

@@ -232,6 +232,8 @@
       <arg name="input/object1" value="clustering/camera_lidar_fusion/objects"/>
       <arg name="output/object" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
       <arg name="priority_mode" value="0"/>
+      <arg name="data_association_matrix_path" value="$(var object_recognition_detection_object_merger_data_association_matrix_param_path)"/>
+      <arg name="distance_threshold_list_path" value="$(var object_recognition_detection_object_merger_distance_threshold_list_path)"/>
     </include>
   </group>
 
@@ -242,6 +244,8 @@
       <arg name="input/object0" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
       <arg name="input/object1" value="detection_by_tracker/objects"/>
       <arg name="output/object" value="$(var merger/output/objects)"/>
+      <arg name="data_association_matrix_path" value="$(var object_recognition_detection_object_merger_data_association_matrix_param_path)"/>
+      <arg name="distance_threshold_list_path" value="$(var object_recognition_detection_object_merger_distance_threshold_list_path)"/>
     </include>
   </group>
 

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

@@ -28,6 +28,9 @@
   <arg name="camera_info7" default="/camera_info7"/>
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
 
+  <!-- radar param -->
+  <arg name="input/radar" default="/sensing/radar/detected_objects"/>
+
   <!-- camera lidar fusion based detection-->
   <group>
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml">
@@ -63,14 +66,22 @@
   <group>
     <push-ros-namespace namespace="radar"/>
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/radar_based_detection.launch.xml">
-      <arg name="output/objects" value="objects"/>
+      <arg name="input/radar" value="$(var input/radar)"/>
+      <arg name="output/objects" value="far_objects"/>
+      <arg name="filter/angle_threshold" value="1.0472"/>
+      <arg name="filter/velocity_threshold" value="3.0"/>
+      <arg name="split/velocity_threshold" value="4.5"/>
+      <arg name="split_range" value="70.0"/>
+      <arg name="clustering/angle_threshold" value="0.174"/>
+      <arg name="clustering/distance_threshold" value="10.0"/>
+      <arg name="clustering/velocity_threshold" value="4.0"/>
     </include>
   </group>
 
-  <!-- camera lidar radar fusion-->
+  <!-- lidar radar fusion-->
   <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml">
     <arg name="input/objects" value="camera_lidar_fusion/objects"/>
-    <arg name="input/radars" value="radar/objects"/>
+    <arg name="input/radars" value="radar/noise_filtered_objects"/>
     <arg name="output/objects" value="objects"/>
   </include>
 </launch>

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

@@ -34,6 +34,7 @@
   <arg name="trust_distance" default="30.0"/>
 
   <!-- radar param -->
+  <arg name="input/radar" default="/sensing/radar/detected_objects"/>
 
   <!-- camera lidar radar fusion based detection-->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_radar_fusion&quot;')">
@@ -63,6 +64,7 @@
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="remove_unknown" value="$(var remove_unknown)"/>
       <arg name="trust_distance" value="$(var trust_distance)"/>
+      <arg name="input/radar" value="$(var input/radar)"/>
     </include>
   </group>
 
@@ -104,6 +106,7 @@
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="input/radar" value="$(var input/radar)"/>
     </include>
   </group>
 
@@ -122,6 +125,7 @@
   <!-- radar based detection-->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;radar&quot;')">
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/radar_based_detection.launch.xml">
+      <arg name="input/radar" value="$(var input/radar)"/>
       <arg name="output/objects" value="objects"/>
     </include>
   </group>

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

@@ -120,6 +120,8 @@
       <arg name="input/object0" value="$(var merger/input/objects)"/>
       <arg name="input/object1" value="clustering/objects"/>
       <arg name="output/object" value="temporary_merged_objects"/>
+      <arg name="data_association_matrix_path" value="$(var object_recognition_detection_object_merger_data_association_matrix_param_path)"/>
+      <arg name="distance_threshold_list_path" value="$(var object_recognition_detection_object_merger_distance_threshold_list_path)"/>
     </include>
   </group>
 
@@ -130,6 +132,8 @@
       <arg name="input/object0" value="temporary_merged_objects"/>
       <arg name="input/object1" value="detection_by_tracker/objects"/>
       <arg name="output/object" value="$(var merger/output/objects)"/>
+      <arg name="data_association_matrix_path" value="$(var object_recognition_detection_object_merger_data_association_matrix_param_path)"/>
+      <arg name="distance_threshold_list_path" value="$(var object_recognition_detection_object_merger_distance_threshold_list_path)"/>
     </include>
   </group>
 

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

@@ -9,6 +9,9 @@
   <arg name="container_name" default="pointcloud_container"/>
   <arg name="score_threshold" default="0.20"/>
 
+  <!-- radar param -->
+  <arg name="input/radar" default="/sensing/radar/detected_objects"/>
+
   <!-- lidar based detection-->
   <group>
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/lidar_based_detection.launch.xml">
@@ -27,14 +30,22 @@
   <group>
     <push-ros-namespace namespace="radar"/>
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/radar_based_detection.launch.xml">
-      <arg name="output/objects" value="objects"/>
+      <arg name="input/radar" value="$(var input/radar)"/>
+      <arg name="output/objects" value="far_objects"/>
+      <arg name="filter/angle_threshold" value="1.0472"/>
+      <arg name="filter/velocity_threshold" value="3.0"/>
+      <arg name="split/velocity_threshold" value="4.5"/>
+      <arg name="split_range" value="70.0"/>
+      <arg name="clustering/angle_threshold" value="0.174"/>
+      <arg name="clustering/distance_threshold" value="10.0"/>
+      <arg name="clustering/velocity_threshold" value="4.0"/>
     </include>
   </group>
 
   <!-- lidar radar fusion-->
   <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml">
     <arg name="input/objects" value="lidar/objects"/>
-    <arg name="input/radars" value="radar/objects"/>
+    <arg name="input/radars" value="radar/noise_filtered_objects"/>
     <arg name="output/objects" value="objects"/>
   </include>
 </launch>

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

@@ -1,12 +1,56 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="output/objects" default="objects"/>
-
-  <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
-    <arg name="input/radar_objects" value="/sensing/radar/front_center/objects_raw"/>
-    <arg name="input/odometry" value="/localization/kinematic_state"/>
-    <arg name="output/radar_detected_objects" value="$(var output/objects)"/>
-    <arg name="update_rate_hz" value="20.0"/>
-    <arg name="use_twist_compensation" value="true"/>
+  <arg name="input/radar" default="/sensing/radar/detected_objects"/>
+  <arg name="output/objects" default="far_objects"/>
+  <arg name="filter/angle_threshold" default="1.0472"/>
+  <arg name="filter/velocity_threshold" default="3.0"/>
+  <arg name="split/velocity_threshold" default="4.5"/>
+  <arg name="split_range" default="70.0"/>
+  <arg name="clustering/angle_threshold" default="0.174"/>
+  <arg name="clustering/distance_threshold" default="10.0"/>
+  <arg name="clustering/velocity_threshold" default="4.0"/>
+  <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_position_filter.param.yaml"/>
+
+  <!-- Detection for far dynamic objects -->
+  <include file="$(find-pkg-share radar_crossing_objects_noise_filter)/launch/radar_crossing_objects_noise_filter.launch.xml">
+    <arg name="input/objects" value="$(var input/radar)"/>
+    <arg name="output/noise_objects" value="noise_objects"/>
+    <arg name="output/filtered_objects" value="noise_filtered_objects"/>
+    <arg name="angle_threshold" value="$(var filter/angle_threshold)"/>
+    <arg name="velocity_threshold" value="$(var filter/velocity_threshold)"/>
+  </include>
+
+  <include file="$(find-pkg-share detected_object_validation)/launch/object_lanelet_filter.launch.xml">
+    <arg name="input/object" value="noise_filtered_objects"/>
+    <arg name="output/object" value="lanelet_filtered_objects"/>
+    <arg name="filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
+  </include>
+
+  <include file="$(find-pkg-share object_velocity_splitter)/launch/object_velocity_splitter.launch.xml">
+    <arg name="input/objects" value="lanelet_filtered_objects"/>
+    <arg name="output/low_speed_objects" value="low_speed_objects"/>
+    <arg name="output/high_speed_objects" value="high_speed_objects"/>
+    <arg name="velocity_threshold" value="$(var split/velocity_threshold)"/>
+  </include>
+
+  <include file="$(find-pkg-share object_range_splitter)/launch/object_range_splitter.launch.xml">
+    <arg name="input/object" value="high_speed_objects"/>
+    <arg name="output/long_range_object" value="far_high_speed_objects"/>
+    <arg name="output/short_range_object" value="near_high_speed_objects"/>
+    <arg name="split_range" value="$(var split_range)"/>
+  </include>
+
+  <include file="$(find-pkg-share radar_object_clustering)/launch/radar_object_clustering.launch.xml">
+    <arg name="input/objects" value="far_high_speed_objects"/>
+    <arg name="output/objects" value="$(var output/objects)"/>
+    <arg name="angle_threshold" value="$(var clustering/angle_threshold)"/>
+    <arg name="distance_threshold" value="$(var clustering/distance_threshold)"/>
+    <arg name="velocity_threshold" value="$(var clustering/velocity_threshold)"/>
+    <arg name="is_fixed_label" value="true"/>
+    <arg name="fixed_label" value="CAR"/>
+    <arg name="is_fixed_size" value="true"/>
+    <arg name="size_x" value="4.0"/>
+    <arg name="size_y" value="1.5"/>
+    <arg name="size_z" value="1.5"/>
   </include>
 </launch>

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -9,6 +9,8 @@
   <arg name="object_recognition_detection_object_position_filter_param_path"/>
   <arg name="object_recognition_detection_pointcloud_map_filter_param_path"/>
   <arg name="object_recognition_prediction_map_based_prediction_param_path"/>
+  <arg name="object_recognition_detection_object_merger_data_association_matrix_param_path"/>
+  <arg name="object_recognition_detection_object_merger_distance_threshold_list_path"/>
   <arg name="object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>

launch/tier4_perception_launch/package.xml

@@ -26,10 +26,14 @@
   <exec_depend>multi_object_tracker</exec_depend>
   <exec_depend>object_merger</exec_depend>
   <exec_depend>object_range_splitter</exec_depend>
+  <exec_depend>object_velocity_splitter</exec_depend>
   <exec_depend>occupancy_grid_map_outlier_filter</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pointcloud_to_laserscan</exec_depend>
   <exec_depend>probabilistic_occupancy_grid_map</exec_depend>
+  <exec_depend>radar_crossing_objects_noise_filter</exec_depend>
+  <exec_depend>radar_fusion_to_detected_object</exec_depend>
+  <exec_depend>radar_object_clustering</exec_depend>
   <exec_depend>shape_estimation</exec_depend>
   <exec_depend>tensorrt_yolo</exec_depend>
   <exec_depend>topic_tools</exec_depend>

localization/yabloc/yabloc_pose_initializer/CMakeLists.txt

@@ -50,7 +50,6 @@ target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus)
 # Semantic segmentation
 install(PROGRAMS
   src/semantic_segmentation/semantic_segmentation_core.py
-  src/semantic_segmentation/semantic_segmentation_node.py
   src/semantic_segmentation/semantic_segmentation_server.py
   DESTINATION lib/${PROJECT_NAME}
 )

localization/yabloc/yabloc_pose_initializer/README.md

@@ -5,6 +5,20 @@ This package contains some nodes related to initial pose estimation.
 - [camera_pose_initializer](#camera_pose_initializer)
 - [semantic_segmentation_server](#semantic_segmentation_server)
 
+Ideally, this package downloads a pre-trained semantic segmentation model during the build and loads it at runtime for initialization.
+However, to handle cases where network connectivity is not available at build time, **the default behavior is not to download the model during build.**
+Even if the model is not downloaded, initialization will still complete, but the accuracy may be compromised.
+
+<!-- cspell: ignore DDOWNLOAD_ARTIFACTS -->
+
+To download the model, please specify `--cmake-args -DDOWNLOAD_ARTIFACTS=ON` to the build command.
+
+```bash
+colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DDOWNLOAD_ARTIFACTS=ON --packages-select yabloc_pose_initializer
+```
+
+For detailed information about the downloaded contents, please consult the `download.cmake` file in this package.
+
 ## Note
 
 This package makes use of external code. The trained files are provided by apollo. The trained files are automatically downloaded when you build.

localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp

@@ -70,7 +70,8 @@ class CameraPoseInitializer : public rclcpp::Node
   PoseCovStamped create_rectified_initial_pose(
     const Eigen::Vector3f & pos, double yaw_angle_rad, const PoseCovStamped & src_msg);
 
-  bool estimate_pose(const Eigen::Vector3f & position, double & yaw_angle_rad, double yaw_std_rad);
+  std::optional<double> estimate_pose(
+    const Eigen::Vector3f & position, const double yaw_angle_rad, const double yaw_std_rad);
 };
 }  // namespace yabloc