Merge branch 'main' into feat/rtdc-skip-parking-calc

codecov.yaml

@@ -31,3 +31,85 @@ ignore:
   - "**/test/*"
   - "**/test/**/*"
   - "**/debug.*"
+
+component_management:
+  individual_components:
+    - component_id: planning-tier-iv-maintained-packages
+      name: Planning TIER IV Maintained Packages
+      paths:
+        - planning/autoware_costmap_generator/**
+        - planning/autoware_external_velocity_limit_selector/**
+        - planning/autoware_freespace_planner/**
+        - planning/autoware_freespace_planning_algorithms/**
+        - planning/autoware_mission_planner/**
+        # - planning/autoware_objects_of_interest_marker_interface/**
+        - planning/autoware_obstacle_cruise_planner/**
+        # - planning/autoware_obstacle_stop_planner/**
+        - planning/autoware_path_optimizer/**
+        - planning/autoware_path_smoother/**
+        - planning/autoware_planning_test_manager/**
+        - planning/autoware_planning_topic_converter/**
+        - planning/autoware_planning_validator/**
+        - planning/autoware_remaining_distance_time_calculator/**
+        - planning/autoware_route_handler/**
+        - planning/autoware_rtc_interface/**
+        - planning/autoware_scenario_selector/**
+        - planning/autoware_static_centerline_generator/**
+        - planning/autoware_surround_obstacle_checker/**
+        - planning/autoware_velocity_smoother/**
+        ##### behavior_path_planner #####
+        # - planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/**
+        - planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/**
+        - planning/behavior_path_planner/autoware_behavior_path_planner_common/**
+        - planning/behavior_path_planner/autoware_behavior_path_start_planner_module/**
+        - planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/**
+        - planning/behavior_path_planner/autoware_behavior_path_lane_change_module/**
+        # - planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/**
+        - planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/**
+        # - planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/**
+        - planning/behavior_path_planner/autoware_behavior_path_planner/**
+        - planning/behavior_path_planner/autoware_behavior_path_side_shift_module/**
+        ##### behavior_velocity_planner #####
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/**
+        # - planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/**
+        # - planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_planner/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/**
+        # - planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/**
+        # - planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/**
+        - planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/**
+        ##### motion_velocity_planner #####
+        - planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/**
+        - planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/**
+        - planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/**
+        - planning/motion_velocity_planner/autoware_motion_velocity_planner_common/**
+        - planning/motion_velocity_planner/autoware_motion_velocity_planner_node/**
+
+    - component_id: control-tier-iv-maintained-packages
+      name: Control TIER IV Maintained Packages
+      paths:
+        - control/autoware_autonomous_emergency_braking/**
+        - control/autoware_control_validator/**
+        - control/autoware_external_cmd_selector/**
+        # - control/autoware_joy_controller/**
+        - control/autoware_lane_departure_checker/**
+        - control/autoware_mpc_lateral_controller/**
+        - control/autoware_operation_mode_transition_manager/**
+        - control/autoware_pid_longitudinal_controller/**
+        # - control/autoware_pure_pursuit/**
+        - control/autoware_shift_decider/**
+        # - control/autoware_smart_mpc_trajectory_follower/**
+        - control/autoware_trajectory_follower_base/**
+        - control/autoware_trajectory_follower_node/**
+        - control/autoware_vehicle_cmd_gate/**
+        # - control/control_performance_analysis/**
+        - control/obstacle_collision_checker/**
+        # - control/predicted_path_checker/**

common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp

@@ -723,7 +723,7 @@ TEST(alt_geometry, within)
 TEST(alt_geometry, areaRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;
-  constexpr auto polygons_nb = 500;
+  constexpr auto polygons_nb = 100;
   constexpr auto max_vertices = 10;
   constexpr auto max_values = 1000;
 
@@ -763,7 +763,7 @@ TEST(alt_geometry, areaRand)
 TEST(alt_geometry, convexHullRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;
-  constexpr auto polygons_nb = 500;
+  constexpr auto polygons_nb = 100;
   constexpr auto max_vertices = 10;
   constexpr auto max_values = 1000;
 
@@ -811,7 +811,7 @@ TEST(alt_geometry, convexHullRand)
 TEST(alt_geometry, coveredByRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;
-  constexpr auto polygons_nb = 500;
+  constexpr auto polygons_nb = 100;
   constexpr auto max_vertices = 10;
   constexpr auto max_values = 1000;
 
@@ -878,7 +878,7 @@ TEST(alt_geometry, coveredByRand)
 TEST(alt_geometry, disjointRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;
-  constexpr auto polygons_nb = 500;
+  constexpr auto polygons_nb = 100;
   constexpr auto max_vertices = 10;
   constexpr auto max_values = 1000;
 
@@ -944,7 +944,7 @@ TEST(alt_geometry, disjointRand)
 TEST(alt_geometry, intersectsRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;
-  constexpr auto polygons_nb = 500;
+  constexpr auto polygons_nb = 100;
   constexpr auto max_vertices = 10;
   constexpr auto max_values = 1000;
 
@@ -1010,7 +1010,7 @@ TEST(alt_geometry, intersectsRand)
 TEST(alt_geometry, touchesRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;
-  constexpr auto polygons_nb = 500;
+  constexpr auto polygons_nb = 100;
   constexpr auto max_vertices = 10;
   constexpr auto max_values = 1000;
 
@@ -1077,7 +1077,7 @@ TEST(alt_geometry, touchesRand)
 TEST(alt_geometry, withinPolygonRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;
-  constexpr auto polygons_nb = 500;
+  constexpr auto polygons_nb = 100;
   constexpr auto max_vertices = 10;
   constexpr auto max_values = 1000;
 

common/autoware_universe_utils/test/src/geometry/test_geometry.cpp

@@ -1937,7 +1937,7 @@ TEST(
 TEST(geometry, intersectPolygonRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;
-  constexpr auto polygons_nb = 500;
+  constexpr auto polygons_nb = 100;
   constexpr auto max_vertices = 10;
   constexpr auto max_values = 1000;
 
@@ -2227,7 +2227,7 @@ TEST(geometry, intersectConcavePolygonRand)
 {
   std::vector<autoware::universe_utils::Polygon2d> polygons;
   std::vector<std::vector<autoware::universe_utils::Polygon2d>> triangulations;
-  constexpr auto polygons_nb = 500;
+  constexpr auto polygons_nb = 100;
   constexpr auto max_vertices = 10;
   constexpr auto max_values = 1000;
 

control/autoware_autonomous_emergency_braking/README.md

@@ -213,7 +213,8 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t
 | cluster_minimum_height             | [m]    | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets                                                          | 0.1           |
 | minimum_cluster_size               | [-]    | int    | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle                                                                                  | 10            |
 | maximum_cluster_size               | [-]    | int    | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle                                                                                           | 10000         |
-| min_generated_path_length          | [m]    | double | minimum distance for a predicted path generated by sensors                                                                                                                                      | 0.5           |
+| min_generated_imu_path_length      | [m]    | double | minimum distance for a predicted path generated by sensors                                                                                                                                      | 0.5           |
+| max_generated_imu_path_length      | [m]    | double | maximum distance for a predicted path generated by sensors                                                                                                                                      | 10.0          |
 | expand_width                       | [m]    | double | expansion width of the ego vehicle for the collision check                                                                                                                                      | 0.1           |
 | longitudinal_offset                | [m]    | double | longitudinal offset distance for collision check                                                                                                                                                | 2.0           |
 | t_response                         | [s]    | double | response time for the ego to detect the front vehicle starting deceleration                                                                                                                     | 1.0           |

control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml

@@ -7,7 +7,8 @@
     use_predicted_object_data: false
     use_object_velocity_calculation: true
     check_autoware_state: true
-    min_generated_path_length: 0.5
+    min_generated_imu_path_length: 0.5
+    max_generated_imu_path_length: 10.0
     imu_prediction_time_horizon: 1.5
     imu_prediction_time_interval: 0.1
     mpc_prediction_time_horizon: 4.5

control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp

@@ -562,7 +562,8 @@ class AEB : public rclcpp::Node
   double voxel_grid_x_;
   double voxel_grid_y_;
   double voxel_grid_z_;
-  double min_generated_path_length_;
+  double min_generated_imu_path_length_;
+  double max_generated_imu_path_length_;
   double expand_width_;
   double longitudinal_offset_;
   double t_response_;

control/autoware_autonomous_emergency_braking/src/node.cpp

@@ -165,7 +165,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
   voxel_grid_x_ = declare_parameter<double>("voxel_grid_x");
   voxel_grid_y_ = declare_parameter<double>("voxel_grid_y");
   voxel_grid_z_ = declare_parameter<double>("voxel_grid_z");
-  min_generated_path_length_ = declare_parameter<double>("min_generated_path_length");
+  min_generated_imu_path_length_ = declare_parameter<double>("min_generated_imu_path_length");
+  max_generated_imu_path_length_ = declare_parameter<double>("max_generated_imu_path_length");
   expand_width_ = declare_parameter<double>("expand_width");
   longitudinal_offset_ = declare_parameter<double>("longitudinal_offset");
   t_response_ = declare_parameter<double>("t_response");
@@ -227,7 +228,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter(
   updateParam<double>(parameters, "voxel_grid_x", voxel_grid_x_);
   updateParam<double>(parameters, "voxel_grid_y", voxel_grid_y_);
   updateParam<double>(parameters, "voxel_grid_z", voxel_grid_z_);
-  updateParam<double>(parameters, "min_generated_path_length", min_generated_path_length_);
+  updateParam<double>(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_);
+  updateParam<double>(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_);
   updateParam<double>(parameters, "expand_width", expand_width_);
   updateParam<double>(parameters, "longitudinal_offset", longitudinal_offset_);
   updateParam<double>(parameters, "t_response", t_response_);
@@ -461,7 +463,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
     const auto ego_polys = generatePathFootprint(path, expand_width_);
     std::vector<ObjectData> objects;
     // Crop out Pointcloud using an extra wide ego path
-    if (use_pointcloud_data_ && !points_belonging_to_cluster_hulls->empty()) {
+    if (
+      use_pointcloud_data_ && points_belonging_to_cluster_hulls &&
+      !points_belonging_to_cluster_hulls->empty()) {
       const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp;
       getClosestObjectsOnPath(path, current_time, points_belonging_to_cluster_hulls, objects);
     }
@@ -637,39 +641,35 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w)
   ini_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0);
   ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw);
   path.push_back(ini_pose);
-
-  if (std::abs(curr_v) < 0.1) {
-    // if current velocity is too small, assume it stops at the same point
+  const double & dt = imu_prediction_time_interval_;
+  const double distance_between_points = curr_v * dt;
+  constexpr double minimum_distance_between_points{1e-2};
+  // if current velocity is too small, assume it stops at the same point
+  // if distance between points is too small, arc length calculation is unreliable, so we skip
+  // creating the path
+  if (std::abs(curr_v) < 0.1 || distance_between_points < minimum_distance_between_points) {
     return path;
   }
 
-  constexpr double epsilon = std::numeric_limits<double>::epsilon();
-  const double & dt = imu_prediction_time_interval_;
   const double & horizon = imu_prediction_time_horizon_;
-  for (double t = 0.0; t < horizon + epsilon; t += dt) {
-    curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt;
-    curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt;
-    curr_yaw = curr_yaw + curr_w * dt;
-    geometry_msgs::msg::Pose current_pose;
-    current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0);
-    current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw);
-    if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) {
-      continue;
-    }
-    path.push_back(current_pose);
-  }
+  double path_arc_length = 0.0;
+  double t = 0.0;
 
-  // If path is shorter than minimum path length
-  while (autoware::motion_utils::calcArcLength(path) < min_generated_path_length_) {
+  bool finished_creating_path = false;
+  while (!finished_creating_path) {
     curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt;
     curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt;
     curr_yaw = curr_yaw + curr_w * dt;
     geometry_msgs::msg::Pose current_pose;
     current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0);
     current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw);
-    if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) {
-      continue;
-    }
+
+    t += dt;
+    path_arc_length += distance_between_points;
+
+    finished_creating_path = (t > horizon) && (path_arc_length > min_generated_imu_path_length_);
+    finished_creating_path =
+      (finished_creating_path) || (path_arc_length > max_generated_imu_path_length_);
     path.push_back(current_pose);
   }
   return path;
@@ -689,12 +689,15 @@ std::optional<Path> AEB::generateEgoPath(const Trajectory & predicted_traj)
   time_keeper_->start_track("createPath");
   Path path;
   path.reserve(predicted_traj.points.size());
+  constexpr double minimum_distance_between_points{1e-2};
   for (size_t i = 0; i < predicted_traj.points.size(); ++i) {
     geometry_msgs::msg::Pose map_pose;
     tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value());
 
     // skip points that are too close to the last point in the path
-    if (autoware::universe_utils::calcDistance2d(path.back(), map_pose) < 1e-2) {
+    if (
+      autoware::universe_utils::calcDistance2d(path.back(), map_pose) <
+      minimum_distance_between_points) {
       continue;
     }
 
@@ -712,6 +715,9 @@ void AEB::generatePathFootprint(
   const Path & path, const double extra_width_margin, std::vector<Polygon2d> & polygons)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  if (path.empty()) {
+    return;
+  }
   for (size_t i = 0; i < path.size() - 1; ++i) {
     polygons.push_back(
       createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin));
@@ -721,8 +727,11 @@ void AEB::generatePathFootprint(
 std::vector<Polygon2d> AEB::generatePathFootprint(
   const Path & path, const double extra_width_margin)
 {
-  std::vector<Polygon2d> polygons;
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  if (path.empty()) {
+    return {};
+  }
+  std::vector<Polygon2d> polygons;
   for (size_t i = 0; i < path.size() - 1; ++i) {
     polygons.push_back(
       createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin));
@@ -735,7 +744,7 @@ void AEB::createObjectDataUsingPredictedObjects(
   std::vector<ObjectData> & object_data_vector)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
-  if (predicted_objects_ptr_->objects.empty()) return;
+  if (predicted_objects_ptr_->objects.empty() || ego_polys.empty()) return;
 
   const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity;
   const auto & objects = predicted_objects_ptr_->objects;
@@ -926,6 +935,9 @@ void AEB::cropPointCloudWithEgoFootprintPath(
   const std::vector<Polygon2d> & ego_polys, PointCloud::Ptr filtered_objects)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  if (ego_polys.empty()) {
+    return;
+  }
   PointCloud::Ptr full_points_ptr(new PointCloud);
   pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr);
   // Create a Point cloud with the points of the ego footprint