[pull] main from autowarefoundation:main

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

@@ -5,7 +5,7 @@ on:
 
 jobs:
   build-and-test-differential:
-    runs-on: ubuntu-latest
+    runs-on: [self-hosted, linux, X64]
     container: ${{ matrix.container }}${{ matrix.container-suffix }}
     strategy:
       fail-fast: false
@@ -59,7 +59,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/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_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_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/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>

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -145,19 +145,16 @@
         longitudinal:
           prepare_time: 2.0                             # [s]
           min_prepare_distance: 1.0                     # [m]
-          min_avoidance_distance: 10.0                  # [m]
-          min_nominal_avoidance_speed: 7.0              # [m/s]
-          min_sharp_avoidance_speed: 1.0                # [m/s]
           min_slow_down_speed: 1.38                     # [m/s]
           buf_slow_down_speed: 0.56                     # [m/s]
+          nominal_avoidance_speed: 8.33                 # [m/s]
 
       # For yield maneuver
       yield:
         yield_velocity: 2.78                            # [m/s]
 
       # For stop maneuver
       stop:
-        min_distance: 10.0                              # [m]
         max_distance: 20.0                              # [m]
         stop_buffer: 1.0                                # [m]
 
@@ -167,9 +164,10 @@
 
         # lateral constraints
         lateral:
-          nominal_lateral_jerk: 0.2                     # [m/sss]
-          max_lateral_jerk: 1.0                         # [m/sss]
-          max_lateral_acceleration: 0.5                 # [m/ss]
+          velocity: [1.0, 1.38, 11.1]                   # [m/s]
+          max_accel_values: [0.5, 0.5, 0.5]             # [m/ss]
+          min_jerk_values: [0.2, 0.2, 0.2]              # [m/sss]
+          max_jerk_values: [1.0, 1.0, 1.0]             # [m/sss]
 
         # longitudinal constraints
         longitudinal:

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -421,12 +421,6 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const;
 
-  /*
-   * @brief trim invalid shift lines whose gradient it too large to follow.
-   * @param target shift lines.
-   */
-  void trimTooSharpShift(AvoidLineArray & shift_lines) const;
-
   /*
    * @brief trim invalid shift lines whose gradient it too large to follow.
    * @param target shift lines.

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -103,7 +103,9 @@ class LaneChangeBase
 
   virtual PathSafetyStatus isApprovedPathSafe() const = 0;
 
-  virtual bool isNearEndOfLane() const = 0;
+  virtual bool isNearEndOfCurrentLanes(
+    const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+    const double threshold) const = 0;
 
   virtual bool getAbortPath() = 0;
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -74,7 +74,9 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isRequiredStop(const bool is_object_coming_from_rear) const override;
 
-  bool isNearEndOfLane() const override;
+  bool isNearEndOfCurrentLanes(
+    const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+    const double threshold) const override;
 
   bool hasFinishedLaneChange() const override;
 
@@ -98,6 +100,10 @@ class NormalLaneChange : public LaneChangeBase
 
   int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override;
 
+  double calcPrepareDuration(
+    const lanelet::ConstLanelets & current_lanes,
+    const lanelet::ConstLanelets & target_lanes) const;
+
   LaneChangeTargetObjects getTargetObjects(
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;