Merge branch 'main' into chore/add_json_schema_to_twist2accel

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

@@ -16,7 +16,7 @@ jobs:
   build-and-test-differential:
     needs: prevent-no-label-execution
     if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }}
-    runs-on: [self-hosted, linux, X64]
+    runs-on: ubuntu-latest
     container: ${{ matrix.container }}${{ matrix.container-suffix }}
     strategy:
       fail-fast: false
@@ -70,7 +70,7 @@ jobs:
           flags: differential
 
   clang-tidy-differential:
-    runs-on: [self-hosted, linux, X64]
+    runs-on: ubuntu-latest
     container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda
     needs: build-and-test-differential
     steps:

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

@@ -31,8 +31,6 @@
 
   <!-- Radar parameters -->
   <arg name="input/radar" default="/sensing/radar/detected_objects"/>
-  <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
-  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- Camera-Lidar-Radar fusion based detection -->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_radar_fusion&quot;')">
@@ -75,11 +73,10 @@
       <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/radar_detector.launch.xml">
         <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="radar_lanelet_filtering_range_param_path" value="$(var radar_lanelet_filtering_range_param_path)"/>
+        <arg name="radar_crossing_objects_noise_filter_param_path" value="$(var object_recognition_detection_radar_crossing_objects_noise_filter_param_path)"/>
         <arg name="object_velocity_splitter_param_path" value="$(var object_recognition_detection_object_velocity_splitter_radar_fusion_param_path)"/>
         <arg name="object_range_splitter_param_path" value="$(var object_recognition_detection_object_range_splitter_radar_fusion_param_path)"/>
-        <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
         <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
       </include>
     </group>
@@ -177,9 +174,10 @@
       <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/radar_detector.launch.xml">
         <arg name="input/radar" value="$(var input/radar)"/>
         <arg name="output/objects" value="far_objects"/>
+        <arg name="radar_lanelet_filtering_range_param_path" value="$(var radar_lanelet_filtering_range_param_path)"/>
+        <arg name="radar_crossing_objects_noise_filter_param_path" value="$(var object_recognition_detection_radar_crossing_objects_noise_filter_param_path)"/>
         <arg name="object_velocity_splitter_param_path" value="$(var object_recognition_detection_object_velocity_splitter_radar_fusion_param_path)"/>
         <arg name="object_range_splitter_param_path" value="$(var object_recognition_detection_object_range_splitter_radar_fusion_param_path)"/>
-        <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
         <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
       </include>
     </group>
@@ -238,9 +236,10 @@
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detector/radar_detector.launch.xml">
       <arg name="input/radar" value="$(var input/radar)"/>
       <arg name="output/objects" value="objects"/>
+      <arg name="radar_lanelet_filtering_range_param_path" value="$(var radar_lanelet_filtering_range_param_path)"/>
+      <arg name="radar_crossing_objects_noise_filter_param_path" value="$(var object_recognition_detection_radar_crossing_objects_noise_filter_param_path)"/>
       <arg name="object_velocity_splitter_param_path" value="$(var object_recognition_detection_object_velocity_splitter_radar_param_path)"/>
       <arg name="object_range_splitter_param_path" value="$(var object_recognition_detection_object_range_splitter_radar_param_path)"/>
-      <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
       <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
     </include>
   </group>

launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml

@@ -2,8 +2,7 @@
 <launch>
   <arg name="input/radar" default="/sensing/radar/detected_objects"/>
   <arg name="output/objects" default="far_objects"/>
-  <arg name="filter/angle_threshold" default="1.221"/>
-  <arg name="filter/velocity_threshold" default="1.5"/>
+  <arg name="radar_crossing_objects_noise_filter_param_path"/>
   <arg name="object_velocity_splitter_param_path" default="$(var object_recognition_detection_object_velocity_splitter_radar_param_path)"/>
   <arg name="object_range_splitter_param_path" default="$(var object_recognition_detection_object_range_splitter_radar_param_path)"/>
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
@@ -14,8 +13,7 @@
     <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)"/>
+    <arg name="param_path" value="$(var radar_crossing_objects_noise_filter_param_path)"/>
   </include>
 
   <include file="$(find-pkg-share object_velocity_splitter)/launch/object_velocity_splitter.launch.xml">
@@ -35,12 +33,12 @@
   <include file="$(find-pkg-share detected_object_validation)/launch/object_lanelet_filter.launch.xml">
     <arg name="input/object" value="far_high_speed_objects"/>
     <arg name="output/object" value="lanelet_filtered_objects"/>
-    <arg name="filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
+    <arg name="filtering_range_param_path" value="$(var radar_lanelet_filtering_range_param_path)"/>
   </include>
 
   <include file="$(find-pkg-share radar_object_clustering)/launch/radar_object_clustering.launch.xml">
     <arg name="input/objects" value="lanelet_filtered_objects"/>
     <arg name="output/objects" value="$(var output/objects)"/>
-    <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
+    <arg name="param_path" value="$(var radar_object_clustering_param_path)"/>
   </include>
 </launch>

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -14,7 +14,8 @@
   <arg name="object_recognition_detection_roi_pointcloud_fusion_param_path"/>
   <arg name="object_recognition_detection_roi_detected_object_fusion_param_path"/>
   <arg name="object_recognition_detection_lidar_model_param_path"/>
-  <arg name="object_recognition_detection_radar_lanelet_filtering_range_param"/>
+  <arg name="object_recognition_detection_radar_lanelet_filtering_range_param_path"/>
+  <arg name="object_recognition_detection_radar_crossing_objects_noise_filter_param_path"/>
   <arg name="object_recognition_detection_radar_object_clustering_param_path"/>
   <arg name="object_recognition_detection_object_velocity_splitter_radar_param_path"/>
   <arg name="object_recognition_detection_object_velocity_splitter_radar_fusion_param_path"/>
@@ -189,7 +190,7 @@
           <arg name="euclidean_param_path" value="$(var object_recognition_detection_euclidean_cluster_param_path)"/>
           <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="radar_lanelet_filtering_range_param_path" value="$(var object_recognition_detection_radar_lanelet_filtering_range_param_path)"/>
           <arg name="radar_object_clustering_param_path" value="$(var object_recognition_detection_radar_object_clustering_param_path)"/>
           <arg name="detection_by_tracker_param_path" value="$(var object_recognition_detection_detection_by_tracker_param)"/>
 

perception/radar_crossing_objects_noise_filter/CMakeLists.txt

@@ -37,4 +37,5 @@ endif()
 ament_auto_package(
   INSTALL_TO_SHARE
   launch
+  config
 )

perception/radar_crossing_objects_noise_filter/config/radar_crossing_objects_noise_filter.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    angle_threshold: 1.2210
+    velocity_threshold: 1.5

perception/radar_crossing_objects_noise_filter/launch/radar_crossing_objects_noise_filter.launch.xml

@@ -5,15 +5,13 @@
   <arg name="output/noise_objects" default="~/output/noise_objects"/>
   <arg name="output/filtered_objects" default="~/output/filtered_objects"/>
   <!-- Parameter -->
-  <arg name="angle_threshold" default="1.0472"/>
-  <arg name="velocity_threshold" default="3.0"/>
+  <arg name="param_path" default="$(find-pkg-share radar_crossing_objects_noise_filter)/config/radar_crossing_objects_noise_filter.param.yaml"/>
 
   <!-- Node -->
   <node pkg="radar_crossing_objects_noise_filter" exec="radar_crossing_objects_noise_filter_node" name="radar_crossing_objects_noise_filter" output="screen">
     <remap from="~/input/objects" to="$(var input/objects)"/>
     <remap from="~/output/noise_objects" to="$(var output/noise_objects)"/>
     <remap from="~/output/filtered_objects" to="$(var output/filtered_objects)"/>
-    <param name="angle_threshold" value="$(var angle_threshold)"/>
-    <param name="velocity_threshold" value="$(var velocity_threshold)"/>
+    <param from="$(var param_path)"/>
   </node>
 </launch>

perception/radar_object_clustering/launch/radar_object_clustering.launch.xml

@@ -1,15 +1,12 @@
 <launch>
-  <!-- Input -->
   <arg name="input/objects" default="~/input/objects"/>
-  <!-- Output -->
   <arg name="output/objects" default="~/output/objects"/>
-  <!-- Parameter -->
-  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
+  <arg name="param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- Node -->
   <node pkg="radar_object_clustering" exec="radar_object_clustering_node" name="radar_object_clustering" output="screen">
     <remap from="~/input/objects" to="$(var input/objects)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
-    <param from="$(var radar_object_clustering_param_path)"/>
+    <param from="$(var param_path)"/>
   </node>
 </launch>

perception/radar_tracks_msgs_converter/CMakeLists.txt

@@ -24,5 +24,6 @@ endif()
 ## Package
 ament_auto_package(
   INSTALL_TO_SHARE
+    config
     launch
 )

perception/radar_tracks_msgs_converter/config/radar_tracks_msgs_converter.param.yaml

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    update_rate_hz: 20.0
+    use_twist_compensation: true
+    use_twist_yaw_compensation: false
+    static_object_speed_threshold: 1.0

perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml

@@ -3,19 +3,13 @@
   <arg name="input/odometry" default="/localization/kinematic_state"/>
   <arg name="output/radar_detected_objects" default="output/radar_detected_objects"/>
   <arg name="output/radar_tracked_objects" default="output/radar_tracked_objects"/>
-  <arg name="update_rate_hz" default="20.0"/>
-  <arg name="use_twist_compensation" default="true"/>
-  <arg name="use_twist_yaw_compensation" default="false"/>
-  <arg name="static_object_speed_threshold" default="1.0"/>
+  <arg name="param_path" default="$(find-pkg-share radar_tracks_msgs_converter)/config/radar_tracks_msgs_converter.param.yaml"/>
 
   <node pkg="radar_tracks_msgs_converter" exec="radar_tracks_msgs_converter_node" name="radar_tracks_msgs_converter" output="screen">
     <remap from="~/input/radar_objects" to="$(var input/radar_objects)"/>
     <remap from="~/input/odometry" to="$(var input/odometry)"/>
     <remap from="~/output/radar_detected_objects" to="$(var output/radar_detected_objects)"/>
     <remap from="~/output/radar_tracked_objects" to="$(var output/radar_tracked_objects)"/>
-    <param name="update_rate_hz" value="$(var update_rate_hz)"/>
-    <param name="use_twist_compensation" value="$(var use_twist_compensation)"/>
-    <param name="use_twist_yaw_compensation" value="$(var use_twist_yaw_compensation)"/>
-    <param name="static_object_speed_threshold" value="$(var static_object_speed_threshold)"/>
+    <param from="$(var param_path)"/>
   </node>
 </launch>

perception/simple_object_merger/CMakeLists.txt

@@ -25,5 +25,6 @@ endif()
 # Package
 ament_auto_package(
   INSTALL_TO_SHARE
+  config
   launch
 )

perception/simple_object_merger/README.md

@@ -72,5 +72,19 @@ If the time difference between the first topic of `input_topics` and an input to
   - Default parameter: "[]"
 
 This parameter is the name of input topics.
-For example, when this packages use for radar objects, `"[/sensing/radar/front_center/detected_objects, /sensing/radar/front_left/detected_objects, /sensing/radar/rear_left/detected_objects, /sensing/radar/rear_center/detected_objects, /sensing/radar/rear_right/detected_objects, /sensing/radar/front_right/detected_objects]"` can be set.
+For example, when this packages use for radar objects,
+
+```yaml
+input_topics:
+  [
+    "/sensing/radar/front_center/detected_objects",
+    "/sensing/radar/front_left/detected_objects",
+    "/sensing/radar/rear_left/detected_objects",
+    "/sensing/radar/rear_center/detected_objects",
+    "/sensing/radar/rear_right/detected_objects",
+    "/sensing/radar/front_right/detected_objects",
+  ]
+```
+
+can be set in config yaml file.
 For now, the time difference is calculated by the header time between the first topic of `input_topics` and the input topics, so the most important objects to detect should be set in the first of `input_topics` list.

perception/simple_object_merger/config/simple_object_merger.param.yaml

@@ -0,0 +1,6 @@
+/**:
+  ros__parameters:
+    update_rate_hz: 20.0
+    new_frame_id: "base_link"
+    timeout_threshold: 1.0
+    input_topics: [""]

perception/simple_object_merger/launch/simple_object_merger.launch.xml

@@ -1,18 +1,9 @@
 <launch>
-  <!-- Output -->
   <arg name="output/objects" default="~/output/objects"/>
-  <!-- Parameter -->
-  <arg name="update_rate_hz" default="20.0"/>
-  <arg name="new_frame_id" default="base_link"/>
-  <arg name="timeout_threshold" default="0.1"/>
-  <arg name="input_topics" default="[]"/>
+  <arg name="param_path" default="$(find-pkg-share simple_object_merger)/config/simple_object_merger.param.yaml"/>
 
-  <!-- Node -->
   <node pkg="simple_object_merger" exec="simple_object_merger_node" name="simple_object_merger" output="screen">
     <remap from="~/output/objects" to="$(var output/objects)"/>
-    <param name="update_rate_hz" value="$(var update_rate_hz)"/>
-    <param name="timeout_threshold" value="$(var timeout_threshold)"/>
-    <param name="new_frame_id" value="$(var new_frame_id)"/>
-    <param name="input_topics" value="$(var input_topics)"/>
+    <param from="$(var param_path)"/>
   </node>
 </launch>

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -164,6 +164,7 @@ class StartPlannerModule : public SceneModuleInterface
 
   bool isModuleRunning() const;
   bool isCurrentPoseOnMiddleOfTheRoad() const;
+  bool isOverlapWithCenterLane() const;
   bool isCloseToOriginalStartPose() const;
   bool hasArrivedAtGoal() const;
   bool isMoving() const;

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -21,6 +21,7 @@
 #include "behavior_path_start_planner_module/util.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 
+#include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <magic_enum.hpp>
@@ -211,7 +212,8 @@ bool StartPlannerModule::receivedNewRoute() const
 
 bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const
 {
-  return parameters_->safety_check_params.enable_safety_check && status_.driving_forward;
+  return parameters_->safety_check_params.enable_safety_check && status_.driving_forward &&
+         !isOverlapWithCenterLane();
 }
 
 bool StartPlannerModule::noMovingObjectsAround() const
@@ -278,6 +280,37 @@ bool StartPlannerModule::isCurrentPoseOnMiddleOfTheRoad() const
   return std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road;
 }
 
+bool StartPlannerModule::isOverlapWithCenterLane() const
+{
+  const Pose & current_pose = planner_data_->self_odometry->pose.pose;
+  const auto current_lanes = utils::getCurrentLanes(planner_data_);
+  const auto local_vehicle_footprint = vehicle_info_.createFootprint();
+  const auto vehicle_footprint =
+    transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(current_pose));
+  for (const auto & current_lane : current_lanes) {
+    std::vector<geometry_msgs::msg::Point> centerline_points;
+    for (const auto & point : current_lane.centerline()) {
+      geometry_msgs::msg::Point center_point = lanelet::utils::conversion::toGeomMsgPt(point);
+      centerline_points.push_back(center_point);
+    }
+
+    for (size_t i = 0; i < centerline_points.size() - 1; ++i) {
+      const auto & p1 = centerline_points.at(i);
+      const auto & p2 = centerline_points.at(i + 1);
+      for (size_t j = 0; j < vehicle_footprint.size() - 1; ++j) {
+        const auto p3 = tier4_autoware_utils::toMsg(vehicle_footprint[j].to_3d());
+        const auto p4 = tier4_autoware_utils::toMsg(vehicle_footprint[j + 1].to_3d());
+        const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4);
+
+        if (intersection.has_value()) {
+          return true;
+        }
+      }
+    }
+  }
+  return false;
+}
+
 bool StartPlannerModule::isCloseToOriginalStartPose() const
 {
   const Pose start_pose = planner_data_->route_handler->getOriginalStartPose();
@@ -935,8 +968,8 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
     pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0,
     std::numeric_limits<double>::max());
 
-  // Set the maximum backward distance less than the distance from the vehicle's base_link to the
-  // lane's rearmost point to prevent lane departure.
+  // Set the maximum backward distance less than the distance from the vehicle's base_link to
+  // the lane's rearmost point to prevent lane departure.
   const double current_arc_length =
     lanelet::utils::getArcCoordinates(pull_out_lanes, start_pose).length;
   const double allowed_backward_distance = std::clamp(
@@ -1224,8 +1257,8 @@ bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const
 {
   const auto & rh = planner_data_->route_handler;
 
-  // Check if the goal and ego are in the same route segment. If not, this is out of scope of this
-  // function. Return false.
+  // Check if the goal and ego are in the same route segment. If not, this is out of scope of
+  // this function. Return false.
   lanelet::ConstLanelet ego_lanelet;
   rh->getClosestLaneletWithinRoute(getEgoPose(), &ego_lanelet);
   const auto is_ego_in_goal_route_section = rh->isInGoalRouteSection(ego_lanelet);

planning/obstacle_stop_planner/src/node.cpp

@@ -1548,6 +1548,8 @@ void ObstacleStopPlannerNode::filterObstacles(
   const PredictedObjects & input_objects, const Pose & ego_pose, const TrajectoryPoints & traj,
   const double dist_threshold, PredictedObjects & filtered_objects)
 {
+  if (traj.size() < 2) return;
+
   filtered_objects.header = input_objects.header;
 
   for (auto & object : input_objects.objects) {

sensing/radar_tracks_noise_filter/CMakeLists.txt

@@ -43,5 +43,6 @@ endif()
 # Package
 ament_auto_package(
   INSTALL_TO_SHARE
+  config
   launch
 )

sensing/radar_tracks_noise_filter/config/radar_tracks_noise_filter.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    velocity_y_threshold: 7.0

sensing/radar_tracks_noise_filter/launch/radar_tracks_noise_filter.launch.xml

@@ -1,17 +1,13 @@
 <launch>
-  <!-- Input -->
   <arg name="input/tracks" default="~/input/tracks"/>
-  <!-- Output -->
   <arg name="output/noise_tracks" default="~/output/noise_tracks"/>
   <arg name="output/filtered_tracks" default="~/output/filtered_tracks"/>
-  <!-- Parameter -->
-  <arg name="velocity_y_threshold" default="7.0"/>
+  <arg name="param_path" default="$(find-pkg-share radar_tracks_noise_filter)/config/object_velocity_splitter.param.yaml"/>
 
-  <!-- Node -->
   <node pkg="radar_tracks_noise_filter" exec="radar_tracks_noise_filter_node" name="radar_tracks_noise_filter" output="screen">
     <remap from="~/input/tracks" to="$(var input/tracks)"/>
     <remap from="~/output/noise_tracks" to="$(var output/noise_tracks)"/>
     <remap from="~/output/filtered_tracks" to="$(var output/filtered_tracks)"/>
-    <param name="velocity_y_threshold" value="$(var velocity_y_threshold)"/>
+    <param from="$(var param_path)"/>
   </node>
 </launch>