Merge pull request #766 from tier4/sync-upstream

chore: sync upstream
tier4 · Aug 25, 2023 · c2b9b17 · c2b9b17
2 parents c8ba613 + b5e2c13
commit c2b9b17
Show file tree

Hide file tree

Showing 17 changed files with 113 additions and 84 deletions.
diff --git a/.cspell-partial.json b/.cspell-partial.json
@@ -1,10 +1,5 @@
 {
-  "ignorePaths": [
-    "**/perception/**",
-    "**/planning/**",
-    "**/sensing/**",
-    "perception/bytetrack/lib/**"
-  ],
+  "ignorePaths": ["**/perception/**", "**/sensing/**", "perception/bytetrack/lib/**"],
   "ignoreRegExpList": [],
   "words": []
 }
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
@@ -15,6 +15,7 @@ common/component_interface_utils/** [email protected] [email protected] m
 common/cuda_utils/** [email protected] [email protected]
 common/fake_test_node/** [email protected] [email protected] [email protected] [email protected]
 common/global_parameter_loader/** [email protected]
+common/glog_component/** [email protected]
 common/goal_distance_calculator/** [email protected]
 common/grid_map_utils/** [email protected]
 common/interpolation/** [email protected] [email protected] [email protected]
@@ -53,9 +54,10 @@ common/trtexec_vendor/** [email protected] [email protected]
 common/tvm_utility/** [email protected] [email protected]
 control/autonomous_emergency_braking/** [email protected] [email protected]
 control/control_performance_analysis/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+control/control_validator/** [email protected] [email protected] [email protected]
 control/external_cmd_selector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 control/joy_controller/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-control/lane_departure_checker/** [email protected]
+control/lane_departure_checker/** [email protected] [email protected]
 control/mpc_lateral_controller/** [email protected] [email protected]
 control/obstacle_collision_checker/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 control/operation_mode_transition_manager/** [email protected] [email protected]
@@ -82,7 +84,6 @@ launch/tier4_vehicle_launch/** [email protected]
 localization/ar_tag_based_localizer/** [email protected] [email protected]
 localization/ekf_localizer/** [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/gyro_odometer/** [email protected] [email protected]
-localization/initial_pose_button_panel/** [email protected] [email protected]
 localization/localization_error_monitor/** [email protected] [email protected]
 localization/ndt_scan_matcher/** [email protected] [email protected] [email protected] [email protected]
 localization/pose2twist/** [email protected] [email protected]
@@ -183,11 +184,10 @@ planning/sampling_based_planner/sampler_common/** [email protected]
 planning/scenario_selector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/static_centerline_optimizer/** [email protected] [email protected]
 planning/surround_obstacle_checker/** [email protected]
-sensing/geo_pos_conv/** [email protected]
 sensing/gnss_poser/** [email protected]
 sensing/image_diagnostics/** [email protected]
 sensing/image_transport_decompressor/** [email protected]
-sensing/imu_corrector/** [email protected]
+sensing/imu_corrector/** [email protected] [email protected] [email protected]
 sensing/livox/livox_tag_filter/** [email protected]
 sensing/pointcloud_preprocessor/** [email protected] [email protected] [email protected]
 sensing/radar_scan_to_pointcloud2/** [email protected] [email protected]

diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md
@@ -163,6 +163,8 @@ First, we divide the target objects into obstacles in the target lane, obstacles
 
 ![object lanes](../image/lane_change/lane_objects.drawio.svg)
 
+Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.
+
 ##### Collision check in prepare phase
 
 The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases.
@@ -177,21 +179,13 @@ When driving on the public road with other vehicles, there exist scenarios where
 
 ```C++
 lane_changing_time = f(shift_length, lat_acceleration, lat_jerk)
-minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_velocity * lane_changing_time + backward_length_buffer_for_end_of_lane
+minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_velocity * lane_changing_time + lane_change_finish_judge_buffer
 ```
 
 The following figure illustrates when the lane is blocked in multiple lane changes cases.
 
 ![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png)
 
-#### Intersection
-
-Lane change in the intersection is prohibited following traffic regulation. Therefore, if the goal is place close passed the intersection, the lane change needs to be completed before ego vehicle enters the intersection region. Similar to the lane blocked case, in case of the path is unsafe, ego vehicle will stop and waits for the dynamic object to pass by.
-
-The following figure illustrate the intersection case.
-
-![intersection](../image/lane_change/lane_change-intersection_case.png)
-
 ### Aborting lane change
 
 The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise.
@@ -215,11 +209,11 @@ while(Lane Following)
           if (Is Abort Condition Satisfied) then (**NO**)
           else (**YES**)
             if (Is Enough margin to retry lane change) then (**YES**)
-              if (Ego not depart from current lane yet) then (**YES**)
+              if (Ego is on lane change prepare phase) then (**YES**)
               :Cancel lane change;
               break
               else (**NO**)
-              if (Can perform abort maneuver) then (**YES**)
+              if (Will the overhang to target lane be less than threshold?) then (**YES**)
               :Perform abort maneuver;
               break
               else (NO)
@@ -270,23 +264,40 @@ The last behavior will also occur if the ego vehicle has departed from the curre
 
 The following parameters are configurable in `lane_change.param.yaml`.
 
-| Name                                     | Unit   | Type   | Description                                                                                          | Default value |
-| :--------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------- | ------------- |
-| `prepare_duration`                       | [m]    | double | The preparation time for the ego vehicle to be ready to perform lane change.                         | 4.0           |
-| `lane_changing_safety_check_duration`    | [m]    | double | The total time that is taken to complete the lane-changing task.                                     | 8.0           |
-| `backward_length_buffer_for_end_of_lane` | [m]    | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change                | 2.0           |
-| `lane_change_finish_judge_buffer`        | [m]    | double | The additional buffer used to confirm lane change process completion                                 | 3.0           |
-| `lane_changing_lateral_jerk`             | [m/s3] | double | Lateral jerk value for lane change path generation                                                   | 0.5           |
-| `minimum_lane_changing_velocity`         | [m/s]  | double | Minimum speed during lane changing process.                                                          | 2.78          |
-| `prediction_time_resolution`             | [s]    | double | Time resolution for object's path interpolation and collision check.                                 | 0.5           |
-| `longitudinal_acceleration_sampling_num` | [-]    | int    | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration | 5             |
-| `lateral_acceleration_sampling_num`      | [-]    | int    | Number of possible lane-changing trajectories that are being influenced by lateral acceleration      | 3             |
-| `max_longitudinal_acc`                   | [-]    | double | maximum longitudinal acceleration for lane change                                                    | 1.0           |
-| `min_longitudinal_acc`                   | [-]    | double | maximum longitudinal deceleration for lane change                                                    | -1.0          |
+| Name                                        | Unit   | Type    | Description                                                                                                     | Default value      |
+| :------------------------------------------ | ------ | ------- | --------------------------------------------------------------------------------------------------------------- | ------------------ |
+| `backward_lane_length`                      | [m]    | double  | The backward length to check incoming objects in lane change target lane.                                       | 200.0              |
+| `prepare_duration`                          | [m]    | double  | The preparation time for the ego vehicle to be ready to perform lane change.                                    | 4.0                |
+| `backward_length_buffer_for_end_of_lane`    | [m]    | double  | The end of lane buffer to ensure ego vehicle has enough distance to start lane change                           | 2.0                |
+| `lane_change_finish_judge_buffer`           | [m]    | double  | The additional buffer used to confirm lane change process completion                                            | 3.0                |
+| `finish_judge_lateral_threshold`            | [m]    | double  | Lateral distance threshold to confirm lane change process completion                                            | 0.2                |
+| `lane_changing_lateral_jerk`                | [m/s3] | double  | Lateral jerk value for lane change path generation                                                              | 0.5                |
+| `minimum_lane_changing_velocity`            | [m/s]  | double  | Minimum speed during lane changing process.                                                                     | 2.78               |
+| `prediction_time_resolution`                | [s]    | double  | Time resolution for object's path interpolation and collision check.                                            | 0.5                |
+| `longitudinal_acceleration_sampling_num`    | [-]    | int     | Number of possible lane-changing trajectories that are being influenced by longitudinal acceleration            | 5                  |
+| `lateral_acceleration_sampling_num`         | [-]    | int     | Number of possible lane-changing trajectories that are being influenced by lateral acceleration                 | 3                  |
+| `object_check_min_road_shoulder_width`      | [m]    | double  | Vehicles around the center line within this distance will be excluded from parking objects                      | 0.5                |
+| `object_shiftable_ratio_threshold`          | [-]    | double  | Vehicles around the center line within this distance ratio will be excluded from parking objects                | 0.6                |
+| `min_length_for_turn_signal_activation`     | [m]    | double  | Turn signal will be activated if the ego vehicle approaches to this length from minimum lane change length      | 10.0               |
+| `length_ratio_for_turn_signal_deactivation` | [-]    | double  | Turn signal will be deactivated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8                |
+| `object_shiftable_ratio_threshold`          | [-]    | double  | Vehicles around the center line within this distance ratio will be excluded from parking objects                | 0.6                |
+| `max_longitudinal_acc`                      | [-]    | double  | maximum longitudinal acceleration for lane change                                                               | 1.0                |
+| `min_longitudinal_acc`                      | [-]    | double  | maximum longitudinal deceleration for lane change                                                               | -1.0               |
+| `lateral_acceleration.velocity`             | [m/s]  | double  | Reference velocity for lateral acceleration calculation (look up table)                                         | [0.0, 4.0, 10.0]   |
+| `lateral_acceleration.min_values`           | [m/ss] | double  | Min lateral acceleration values corresponding to velocity (look up table)                                       | [0.15, 0.15, 0.15] |
+| `lateral_acceleration.max_values`           | [m/ss] | double  | Max lateral acceleration values corresponding to velocity (look up table)                                       | [0.5, 0.5, 0.5]    |
+| `target_object.car`                         | [-]    | boolean | Include car objects for safety check                                                                            | true               |
+| `target_object.truck`                       | [-]    | boolean | Include truck objects for safety check                                                                          | true               |
+| `target_object.bus`                         | [-]    | boolean | Include bus objects for safety check                                                                            | true               |
+| `target_object.trailer`                     | [-]    | boolean | Include trailer objects for safety check                                                                        | true               |
+| `target_object.unknown`                     | [-]    | boolean | Include unknown objects for safety check                                                                        | true               |
+| `target_object.bicycle`                     | [-]    | boolean | Include bicycle objects for safety check                                                                        | true               |
+| `target_object.motorcycle`                  | [-]    | boolean | Include motorcycle objects for safety check                                                                     | true               |
+| `target_object.pedestrian`                  | [-]    | boolean | Include pedestrian objects for safety check                                                                     | true               |
 
 ### Collision checks during lane change
 
-The following parameters are configurable in `behavior_path_planner.param.yaml`.
+The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`.
 
 | Name                                       | Unit    | Type    | Description                                                                                                                                                    | Default value |
 | :----------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- |

diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -513,6 +513,12 @@ class AvoidanceModule : public SceneModuleInterface
 
     unlockNewModuleLaunch();
 
+    if (!path_shifter_.getShiftLines().empty()) {
+      left_shift_array_.clear();
+      right_shift_array_.clear();
+      removeRTCStatus();
+    }
+
     current_raw_shift_lines_.clear();
     registered_raw_shift_lines_.clear();
     path_shifter_.setShiftLines(ShiftLineArray{});

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -602,7 +602,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
     case AvoidanceState::YIELD: {
       insertYieldVelocity(path);
       insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
-      removeRegisteredShiftLines();
       break;
     }
     case AvoidanceState::AVOID_PATH_NOT_READY: {
@@ -2115,6 +2114,10 @@ BehaviorModuleOutput AvoidanceModule::plan()
 
   updatePathShifter(data.safe_new_sl);
 
+  if (data.yield_required) {
+    removeRegisteredShiftLines();
+  }
+
   // generate path with shift points that have been inserted.
   ShiftedPath linear_shift_path = utils::avoidance::toShiftedPath(data.reference_path);
   ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path);
@@ -2546,12 +2549,14 @@ void AvoidanceModule::updateRTCData()
 
   updateRegisteredRTCStatus(helper_.getPreviousSplineShiftPath().path);
 
-  if (data.safe_new_sl.empty()) {
+  const auto candidates = data.safe ? data.safe_new_sl : data.unapproved_new_sl;
+
+  if (candidates.empty()) {
     removeCandidateRTCStatus();
     return;
   }
 
-  const auto shift_line = helper_.getMainShiftLine(data.safe_new_sl);
+  const auto shift_line = helper_.getMainShiftLine(candidates);
   if (helper_.getRelativeShiftToPath(shift_line) > 0.0) {
     removePreviousRTCStatusRight();
   } else if (helper_.getRelativeShiftToPath(shift_line) < 0.0) {
@@ -2562,8 +2567,8 @@ void AvoidanceModule::updateRTCData()
 
   CandidateOutput output;
 
-  const auto sl_front = data.safe_new_sl.front();
-  const auto sl_back = data.safe_new_sl.back();
+  const auto sl_front = candidates.front();
+  const auto sl_back = candidates.back();
 
   output.path_candidate = data.candidate_path.path;
   output.lateral_shift = helper_.getRelativeShiftToPath(shift_line);

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -645,10 +645,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
   const auto & route_handler = *getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
   const auto & objects = *planner_data_->dynamic_object;
-  const auto & object_check_min_road_shoulder_width =
-    lane_change_parameters_->object_check_min_road_shoulder_width;
-  const auto & object_shiftable_ratio_threshold =
-    lane_change_parameters_->object_shiftable_ratio_threshold;
 
   // Guard
   if (objects.objects.empty()) {
@@ -709,9 +705,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
     // check if the object intersects with target lanes
     if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
       // ignore static parked object that are in front of the ego vehicle in target lanes
-      bool is_parked_object = utils::lane_change::isParkedObject(
-        target_path, route_handler, extended_object, object_check_min_road_shoulder_width,
-        object_shiftable_ratio_threshold);
+      bool is_parked_object =
+        utils::lane_change::isParkedObject(target_path, route_handler, extended_object, 0.0, 0.0);
       if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) {
         continue;
       }