[pull] main from autowarefoundation:main

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -24,6 +24,7 @@
 #include <algorithm>
 #include <limits>
 #include <stdexcept>
+#include <utility>
 #include <vector>
 
 namespace motion_utils
@@ -580,6 +581,43 @@ double calcArcLength(const T & points)
   return calcSignedArcLength(points, 0, points.size() - 1);
 }
 
+template <class T>
+inline std::vector<double> calcCurvature(const T & points)
+{
+  std::vector<double> curvature_vec(points.size());
+
+  for (size_t i = 1; i < points.size() - 1; ++i) {
+    const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1));
+    const auto p2 = tier4_autoware_utils::getPoint(points.at(i));
+    const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1));
+    curvature_vec.at(i) = (tier4_autoware_utils::calcCurvature(p1, p2, p3));
+  }
+  curvature_vec.at(0) = curvature_vec.at(1);
+  curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2);
+
+  return curvature_vec;
+}
+
+template <class T>
+inline std::vector<std::pair<double, double>> calcCurvatureAndArcLength(const T & points)
+{
+  // Note that arclength is for the segment, not the sum.
+  std::vector<std::pair<double, double>> curvature_arc_length_vec;
+  curvature_arc_length_vec.push_back(std::pair(0.0, 0.0));
+  for (size_t i = 1; i < points.size() - 1; ++i) {
+    const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1));
+    const auto p2 = tier4_autoware_utils::getPoint(points.at(i));
+    const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1));
+    const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3);
+    const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) +
+                              tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1));
+    curvature_arc_length_vec.push_back(std::pair(curvature, arc_length));
+  }
+  curvature_arc_length_vec.push_back(std::pair(0.0, 0.0));
+
+  return curvature_arc_length_vec;
+}
+
 /**
  * @brief Calculate distance to the forward stop point from the given src index
  */

common/motion_utils/package.xml

@@ -8,7 +8,11 @@
   <maintainer email="satoshi.ota@tier4.jp">Satoshi Ota</maintainer>
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <!-- reviewer-->
+  <maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
+  <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
   <maintainer email="taiki.tanaka@tier4.jp">Taiki Tanaka</maintainer>
+  <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
+  <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
   <license>Apache License 2.0</license>
 
   <author email="yutaka.shimizu@tier4.jp">Yutaka Shimizu</author>

common/tensorrt_common/CMakeLists.txt

@@ -4,6 +4,9 @@ project(tensorrt_common)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+# TODO(tensorrt_common): Remove once upgrading to TensorRT 8.5 is complete
+add_compile_options(-Wno-deprecated-declarations)
+
 find_package(CUDA)
 find_package(CUDNN)
 find_package(TENSORRT)

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

@@ -246,6 +246,7 @@
       <arg name="input/object0" value="$(var merger/input/objects)"/>
       <arg name="input/object1" value="clustering/camera_lidar_fusion/short_range_objects"/>
       <arg name="output/object" value="camera_lidar_fusion/objects"/>
+      <arg name="priority_mode" value="0"/>
     </include>
   </group>
 

perception/lidar_centerpoint/CMakeLists.txt

@@ -4,6 +4,9 @@ project(lidar_centerpoint)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+# TODO(lidar_centerpoint): Remove once upgrading to TensorRT 8.5 is complete
+add_compile_options(-Wno-deprecated-declarations)
+
 option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF)
 
 # set flags for CUDA availability
@@ -107,12 +110,12 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL)
   endfunction()
 
   # centerpoint
-  download(v1 pts_voxel_encoder_centerpoint.onnx 94dcf01ccd53ef14a79787afaada63b4)
-  download(v1 pts_backbone_neck_head_centerpoint.onnx 7046995e502e11f35c9459668d8afde3)
+  download(v2 pts_voxel_encoder_centerpoint.onnx e8369d7e0f59951cf87c0274830de1fe)
+  download(v2 pts_backbone_neck_head_centerpoint.onnx 44e44d26781a69c741bf9bddde57fbb3)
 
   # centerpoint_tiny
-  download(v1 pts_voxel_encoder_centerpoint_tiny.onnx 410f730c537968cb27fbd70c941849a8)
-  download(v1 pts_backbone_neck_head_centerpoint_tiny.onnx e97c165c7877222c0e27e44409a07517)
+  download(v2 pts_voxel_encoder_centerpoint_tiny.onnx 5fb774fac44dc83949d05eb3ba077309)
+  download(v2 pts_backbone_neck_head_centerpoint_tiny.onnx e4658325b70222f7c3637fe00e586b82)
 
   find_package(ament_cmake_auto REQUIRED)
   ament_auto_find_build_dependencies()

perception/lidar_centerpoint/README.md

@@ -52,6 +52,12 @@ We trained the models using <https://github.com/open-mmlab/mmdetection3d>.
 
 You can download the onnx format of trained models by clicking on the links below.
 
+- Centerpoint : [pts_voxel_encoder_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint.onnx), [pts_backbone_neck_head_centerpoint.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint.onnx)
+- Centerpoint tiny: [pts_voxel_encoder_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_voxel_encoder_centerpoint_tiny.onnx), [pts_backbone_neck_head_centerpoint_tiny.onnx](https://awf.ml.dev.web.auto/perception/models/centerpoint/v2/pts_backbone_neck_head_centerpoint_tiny.onnx)
+
+`Centerpoint` was trained in `nuScenes` (~110k lidar frames) [8] and TIER IV's internal database (~11k lidar frames) for 60 epochs.
+`Centerpoint tiny` was trained in `Argoverse 2` (~28k lidar frames) [9] and TIER IV's internal database (~11k lidar frames) for 20 epochs.
+
 ## Standalone inference and visualization
 
 In addition to its use as a standard ROS node, `lidar_centerpoint` can also be used to perform inferences in an isolated manner.
@@ -119,6 +125,10 @@ Example:
 
 [7] <https://github.com/NVIDIA-AI-IOT/CUDA-PointPillars>
 
+[8] <https://www.nuscenes.org/nuscenes>
+
+[9] <https://www.argoverse.org/av2.html>
+
 ## (Optional) Future extensions / Unimplemented parts
 
 <!-- Write future extensions of this package.

perception/tensorrt_yolo/CMakeLists.txt

@@ -4,6 +4,9 @@ project(tensorrt_yolo)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+# TODO(tensorrt_yolo): Remove once upgrading to TensorRT 8.5 is complete
+add_compile_options(-Wno-deprecated-declarations)
+
 option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF)
 
 find_package(OpenCV REQUIRED)

perception/traffic_light_ssd_fine_detector/CMakeLists.txt

@@ -4,6 +4,9 @@ project(traffic_light_ssd_fine_detector)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
+# TODO(traffic_light_ssd_fine_detector): Remove once upgrading to TensorRT 8.5 is complete
+add_compile_options(-Wno-deprecated-declarations)
+
 option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF)
 
 find_package(OpenCV REQUIRED)

planning/behavior_path_planner/README.md

@@ -271,15 +271,14 @@ The lateral jerk is searched for among the predetermined minimum and maximum val
 
 ###### Parameters for shift parking
 
-| Name                               | Unit   | Type   | Description                                                                                              | Default value |
-| :--------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ |
-| enable_shift_parking               | [-]    | bool   | flag whether to enable shift parking                                                                     | true          |
-| pull_over_sampling_num             | [-]    | int    | Number of samplings in the minimum to maximum range of lateral_jerk                                      | 4             |
-| maximum_lateral_jerk               | [m/s3] | double | maximum lateral jerk                                                                                     | 2.0           |
-| minimum_lateral_jerk               | [m/s3] | double | minimum lateral jerk                                                                                     | 0.5           |
-| deceleration_interval              | [m]    | double | distance of deceleration section                                                                         | 15.0          |
-| after_pull_over_straight_distance  | [m]    | double | straight line distance after pull over end point                                                         | 5.0           |
-| before_pull_over_straight_distance | [m]    | double | straight line distance before pull over end point. a safe path should have a distance that includes this | 5.0           |
+| Name                              | Unit   | Type   | Description                                                         | Default value |
+| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ |
+| enable_shift_parking              | [-]    | bool   | flag whether to enable shift parking                                | true          |
+| pull_over_sampling_num            | [-]    | int    | Number of samplings in the minimum to maximum range of lateral_jerk | 4             |
+| maximum_lateral_jerk              | [m/s3] | double | maximum lateral jerk                                                | 2.0           |
+| minimum_lateral_jerk              | [m/s3] | double | minimum lateral jerk                                                | 0.5           |
+| deceleration_interval             | [m]    | double | distance of deceleration section                                    | 15.0          |
+| after_pull_over_straight_distance | [m]    | double | straight line distance after pull over end point                    | 5.0           |
 
 ##### **geometric parallel parking**
 
@@ -362,13 +361,13 @@ The Pull Out module is activated when ego-vehicle is stationary and footprint of
 
 #### General parameters for pull_out
 
-| Name                         | Unit  | Type   | Description                                                    | Default value |
-| :--------------------------- | :---- | :----- | :------------------------------------------------------------- | :------------ |
-| th_arrived_distance_m        | [m]   | double | distance threshold for arrival of path termination             | 1.0           |
-| th_stopped_velocity_mps      | [m/s] | double | velocity threshold for arrival of path termination             | 0.01          |
-| th_stopped_time_sec          | [s]   | double | time threshold for arrival of path termination                 | 1.0           |
-| collision_check_margin       | [m]   | double | Obstacle collision check margin                                | 1.0           |
-| pull_out_finish_judge_buffer | [m]   | double | threshold for finish judgment distance from pull out end point | 1.0           |
+| Name                              | Unit  | Type   | Description                                                          | Default value |
+| :-------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ |
+| th_arrived_distance_m             | [m]   | double | distance threshold for arrival of path termination                   | 1.0           |
+| th_stopped_velocity_mps           | [m/s] | double | velocity threshold for arrival of path termination                   | 0.01          |
+| th_stopped_time_sec               | [s]   | double | time threshold for arrival of path termination                       | 1.0           |
+| collision_check_margin            | [m]   | double | Obstacle collision check margin                                      | 1.0           |
+| collision_check_distance_from_end | [m]   | double | collision check distance from end point. currently only for pull out | 15.0          |
 
 #### **Safe check with obstacles in shoulder lane**
 
@@ -396,15 +395,14 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral
 
 ###### parameters for shift pull out
 
-| Name                              | Unit   | Type   | Description                                                                                                          | Default value |
-| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ |
-| enable_shift_pull_out             | -      | bool   | flag whether to enable shift pull out                                                                                | true          |
-| shift_pull_out_velocity           | [m/s]  | double | velocity of shift pull out                                                                                           | 2.0           |
-| pull_out_sampling_num             | -      | int    | Number of samplings in the minimum to maximum range of lateral_jerk                                                  | 4             |
-| before_pull_out_straight_distance | [m]    | double | distance before pull out start point                                                                                 | 0.0           |
-| maximum_lateral_jerk              | [m/s3] | double | maximum lateral jerk                                                                                                 | 2.0           |
-| minimum_lateral_jerk              | [m/s3] | double | minimum lateral jerk                                                                                                 | 0.5           |
-| minimum_shift_pull_out_distance   | [m]    | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0          |
+| Name                            | Unit   | Type   | Description                                                                                                          | Default value |
+| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ |
+| enable_shift_pull_out           | -      | bool   | flag whether to enable shift pull out                                                                                | true          |
+| shift_pull_out_velocity         | [m/s]  | double | velocity of shift pull out                                                                                           | 2.0           |
+| pull_out_sampling_num           | -      | int    | Number of samplings in the minimum to maximum range of lateral_jerk                                                  | 4             |
+| maximum_lateral_jerk            | [m/s3] | double | maximum lateral jerk                                                                                                 | 2.0           |
+| minimum_lateral_jerk            | [m/s3] | double | minimum lateral jerk                                                                                                 | 0.5           |
+| minimum_shift_pull_out_distance | [m]    | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0          |
 
 ##### **geometric pull out**
 

planning/behavior_path_planner/config/pull_out/pull_out.param.yaml

@@ -5,12 +5,11 @@
       th_stopped_velocity: 0.01
       th_stopped_time: 1.0
       collision_check_margin: 1.0
-      pull_out_finish_judge_buffer: 1.0
+      collision_check_distance_from_end: 1.0
       # shift pull out
       enable_shift_pull_out: true
       shift_pull_out_velocity: 2.0
       pull_out_sampling_num: 4
-      before_pull_out_straight_distance: 0.0
       minimum_shift_pull_out_distance: 20.0
       maximum_lateral_jerk: 2.0
       minimum_lateral_jerk: 0.5

planning/behavior_path_planner/config/pull_over/pull_over.param.yaml

@@ -35,7 +35,6 @@
       minimum_lateral_jerk: 0.5
       deceleration_interval: 15.0
       after_pull_over_straight_distance: 5.0
-      before_pull_over_straight_distance: 5.0
       # parallel parking path
       enable_arc_forward_parking: true
       enable_arc_backward_parking: true

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -112,6 +112,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   tier4_autoware_utils::SelfPoseListener self_pose_listener_{this};
   Scenario::SharedPtr current_scenario_{nullptr};
 
+  HADMapBin::ConstSharedPtr map_ptr_{nullptr};
+  LaneletRoute::ConstSharedPtr route_ptr_{nullptr};
+  bool has_received_map_{false};
+  bool has_received_route_{false};
+
   TurnSignalDecider turn_signal_decider_;
 
   std::mutex mutex_pd_;  // mutex for planner_data_
@@ -120,6 +125,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // setup
   bool isDataReady();
 
+  // update planner data
+  std::shared_ptr<PlannerData> createLatestPlannerData();
+
   // parameters
   std::shared_ptr<AvoidanceParameters> avoidance_param_ptr;
   std::shared_ptr<LaneChangeParameters> lane_change_param_ptr;