From c8ee0d4a37a541ee0f3e8e754f40689eff2ab628 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 24 Aug 2023 14:27:54 +0900 Subject: [PATCH 01/10] docs(lane_change): add missing explanation for lane change parameters (#4703) * docs(lane_change): add missing explanation for lane change parameters Signed-off-by: Fumiya Watanabe * docs(lane_change): fix spell Signed-off-by: Fumiya Watanabe * docs(lane_change): add turn signal parameters Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- ...ehavior_path_planner_lane_change_design.md | 45 +++++++++++++------ 1 file changed, 31 insertions(+), 14 deletions(-) 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 index b793173285629..dff4fa9bdc5eb 100644 --- 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 @@ -270,23 +270,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 acitvated 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 deacitvated 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 | | :----------------------------------------- | ------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | From ae24eaff3d4add9785532e0beba8ed1a95c1f74a Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 24 Aug 2023 16:44:13 +0900 Subject: [PATCH 02/10] docs(lane_change): fix document for lane change (#4732) * docs(lane_change): fix document for lane change Signed-off-by: Fumiya Watanabe * docs(lane_change): fix typo Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- ...ehavior_path_planner_lane_change_design.md | 20 +++++++------------ 1 file changed, 7 insertions(+), 13 deletions(-) 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 index dff4fa9bdc5eb..fddd535931d94 100644 --- 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) @@ -284,8 +278,8 @@ The following parameters are configurable in `lane_change.param.yaml`. | `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 acitvated 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 deacitvated if the ego vehicle approaches to this length ratio for lane change finish point | 0.8 | +| `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 | From febb989350a5a2f37cc41b37279c5211648f4993 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 24 Aug 2023 17:47:46 +0900 Subject: [PATCH 03/10] chore: fix cspell errors and enable spell-check in planning directories (#4716) * chore(cspell): change variable name to avoid cspell errors. prevs => previous_lanelets Signed-off-by: Kotaro Yoshimoto * chore(cspell): fix comment to avoid a cspell error Signed-off-by: Kotaro Yoshimoto * chore(cspell): fix comment to avoid a cspell error, dists => distances Signed-off-by: Kotaro Yoshimoto * chore(cspell): fix comment to avoid a cspell error, dsts => distances Signed-off-by: Kotaro Yoshimoto * chore(cspell): entrying => entering Signed-off-by: Kotaro Yoshimoto * refactor: fix typo Descritized => Discretized Signed-off-by: Kotaro Yoshimoto * feat(cspell): enable spell-check for planning directories Signed-off-by: Kotaro Yoshimoto * style(pre-commit): autofix --------- Signed-off-by: Kotaro Yoshimoto Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .cspell-partial.json | 7 +---- .../src/scene_intersection.cpp | 2 +- .../src/scene_intersection.hpp | 4 +-- .../src/util.cpp | 22 +++++++------- .../src/util.hpp | 2 +- .../src/util_type.hpp | 4 +-- .../velocity_planning_utils.cpp | 30 +++++++++---------- .../include/obstacle_cruise_planner/node.hpp | 2 +- 8 files changed, 34 insertions(+), 39 deletions(-) diff --git a/.cspell-partial.json b/.cspell-partial.json index a3c4ca455d444..45b85e779c39d 100644 --- 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/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index d9ebafbe682a0..7ae549ce8b274 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1166,7 +1166,7 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, + const std::vector & lane_divisions, const std::vector & parked_attention_objects, const double occlusion_dist_thr) { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 9d1fab1339d0e..774b68710be7f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -219,7 +219,7 @@ class IntersectionModule : public SceneModuleInterface // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_; + std::optional> occlusion_attention_divisions_; // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop @@ -267,7 +267,7 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, + const std::vector & lane_divisions, const std::vector & parked_attention_objects, const double occlusion_dist_thr); diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 7fdae3df2d9d2..a987ce9af9dfe 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -709,7 +709,7 @@ bool isTrafficLightArrowActivated( return false; } -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) @@ -778,15 +778,15 @@ std::vector generateDetectionLaneDivisions( auto & branch = branches[(ind2id[src])]; int node_iter = ind2id[src]; while (true) { - const auto & dsts = adjacency[(id2ind[node_iter])]; + const auto & destinations = adjacency[(id2ind[node_iter])]; // NOTE: assuming detection lanelets have only one previous lanelet - const auto next = std::find(dsts.begin(), dsts.end(), true); - if (next == dsts.end()) { + const auto next = std::find(destinations.begin(), destinations.end(), true); + if (next == destinations.end()) { branch.push_back(node_iter); break; } branch.push_back(node_iter); - node_iter = ind2id[std::distance(dsts.begin(), next)]; + node_iter = ind2id[std::distance(destinations.begin(), next)]; } } for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { @@ -818,9 +818,9 @@ std::vector generateDetectionLaneDivisions( } // (3) discretize each merged lanelet - std::vector detection_divisions; + std::vector detection_divisions; for (const auto & [last_lane_id, branch] : merged_branches) { - DescritizedLane detection_division; + DiscretizedLane detection_division; detection_division.lane_id = last_lane_id; const auto detection_lanelet = branch.first; const double area = branch.second; @@ -1118,14 +1118,14 @@ void IntersectionLanelets::update( static lanelet::ConstLanelets getPrevLanelets( const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) { - lanelet::ConstLanelets prevs; + lanelet::ConstLanelets previous_lanelets; for (const auto & ll : lanelets_on_path) { if (associative_ids.find(ll.id()) != associative_ids.end()) { - return prevs; + return previous_lanelets; } - prevs.push_back(ll); + previous_lanelets.push_back(ll); } - return prevs; + return previous_lanelets; } std::optional generatePathLanelets( diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index e125f485a65e8..287a75979927b 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -114,7 +114,7 @@ bool isTrafficLightArrowActivated( lanelet::ConstLanelet lane, const std::map & tl_infos); -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 4d2cf9695b2db..26187d75ff53c 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -115,7 +115,7 @@ struct IntersectionLanelets std::optional first_attention_area_ = std::nullopt; }; -struct DescritizedLane +struct DiscretizedLane { int lane_id; // discrete fine lines from left to right @@ -135,7 +135,7 @@ struct IntersectionStopLines struct PathLanelets { lanelet::ConstLanelets prev; - // lanelet::Constlanelet entry2ego; this is included in `all` if exists + // lanelet::ConstLanelet entry2ego; this is included in `all` if exists lanelet::ConstLanelet ego_or_entry2exit; // this is `assigned lane` part of the path(not from // ego) if ego is before the intersection, otherwise from ego to exit diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 3d72d6e4b5c83..2bcb31ff10969 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -222,38 +222,38 @@ bool calcStopVelocityWithConstantJerkAccLimit( return true; } - double dist = 0.0; - std::vector dists; - dists.push_back(dist); + double distance = 0.0; + std::vector distances; + distances.push_back(distance); for (size_t i = start_index; i < output_trajectory.size() - 1; ++i) { - dist += + distance += tier4_autoware_utils::calcDistance2d(output_trajectory.at(i), output_trajectory.at(i + 1)); - if (dist > xs.back()) { + if (distance > xs.back()) { break; } - dists.push_back(dist); + distances.push_back(distance); } if ( - !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(dists) || - !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(dists)) { + !interpolation_utils::isIncreasing(xs) || !interpolation_utils::isIncreasing(distances) || + !interpolation_utils::isNotDecreasing(xs) || !interpolation_utils::isNotDecreasing(distances)) { return false; } if ( - xs.size() < 2 || vs.size() < 2 || as.size() < 2 || js.size() < 2 || dists.empty() || - dists.front() < xs.front() || xs.back() < dists.back()) { + xs.size() < 2 || vs.size() < 2 || as.size() < 2 || js.size() < 2 || distances.empty() || + distances.front() < xs.front() || xs.back() < distances.back()) { return false; } - const auto vel_at_wp = interpolation::lerp(xs, vs, dists); - const auto acc_at_wp = interpolation::lerp(xs, as, dists); - const auto jerk_at_wp = interpolation::lerp(xs, js, dists); + const auto vel_at_wp = interpolation::lerp(xs, vs, distances); + const auto acc_at_wp = interpolation::lerp(xs, as, distances); + const auto jerk_at_wp = interpolation::lerp(xs, js, distances); // for debug std::stringstream ssi; - for (unsigned int i = 0; i < dists.size(); ++i) { - ssi << "d: " << dists.at(i) << ", v: " << vel_at_wp.at(i) << ", a: " << acc_at_wp.at(i) + for (unsigned int i = 0; i < distances.size(); ++i) { + ssi << "d: " << distances.at(i) << ", v: " << vel_at_wp.at(i) << ", a: " << acc_at_wp.at(i) << ", j: " << jerk_at_wp.at(i) << std::endl; } RCLCPP_DEBUG( diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 1927b266b0186..260a4c6400588 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -235,7 +235,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node } void reset(const std::string & uuid) { counter_.emplace(uuid, 0); } - // NOTE: positive is for meeting entrying condition, and negative is for exiting. + // NOTE: positive is for meeting entering condition, and negative is for exiting. std::unordered_map counter_; std::vector current_uuids_; }; From 80583a68f45c946fd08ad622aec49612c00fb353 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 24 Aug 2023 17:51:34 +0900 Subject: [PATCH 04/10] chore(imu_corrector): add maintainer (#4736) Signed-off-by: kminoda --- sensing/imu_corrector/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index bd067e00e4c4e..07c6cfd9a570d 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -5,6 +5,8 @@ 1.0.0 The ROS 2 imu_corrector package Yamato Ando + Taiki Yamada + Koji Minoda Apache License 2.0 Yamato Ando From 25cc92248d42be25031c35920036ce9bbba900fe Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Thu, 24 Aug 2023 18:31:21 +0900 Subject: [PATCH 05/10] chore: update CODEOWNERS (#4594) Signed-off-by: GitHub Co-authored-by: kminoda --- .github/CODEOWNERS | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 4b1727e8a67b1..fb6df15ab7679 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -15,6 +15,7 @@ common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp m common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp +common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @@ -53,9 +54,10 @@ common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -control/lane_departure_checker/** kyoichi.sugahara@tier4.jp +control/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @@ -82,7 +84,6 @@ launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp localization/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/initial_pose_button_panel/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/localization_error_monitor/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp @@ -183,11 +184,10 @@ planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/surround_obstacle_checker/** satoshi.ota@tier4.jp -sensing/geo_pos_conv/** yamato.ando@tier4.jp sensing/gnss_poser/** yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp -sensing/imu_corrector/** yamato.ando@tier4.jp +sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** ryohsuke.mitsudome@tier4.jp sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp From 1e68b9167270e4f644a0e9a3e6b0577b0ec98c02 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 24 Aug 2023 20:25:45 +0900 Subject: [PATCH 06/10] fix(costmap_generator): prevent invalid access for current_pose (#4696) Signed-off-by: tomoya.kimura --- .../nodes/costmap_generator/costmap_generator_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index 381b8b472adfa..afe345490201b 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -350,6 +350,7 @@ bool CostmapGenerator::isActive() return false; } else { const auto & current_pose_wrt_map = getCurrentPose(tf_buffer_, this->get_logger()); + if (!current_pose_wrt_map) return false; return isInParkingLot(lanelet_map_, current_pose_wrt_map->pose); } } From 2624ad018a2153b710e39354969457c99589f1dd Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 24 Aug 2023 21:25:34 +0900 Subject: [PATCH 07/10] fix(lane_change): skip safety check for stopping vehicles (#4733) Signed-off-by: Fumiya Watanabe --- .../src/scene_module/lane_change/normal.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) 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 index 8fdb79a6271d1..3db3e049c3c81 100644 --- 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; } From be626b77544184cccc446e751795e069269fe0fb Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Thu, 24 Aug 2023 22:15:58 +0900 Subject: [PATCH 08/10] fix(motion_velocity_smoother): fix the arg name of the parameter (#4740) Signed-off-by: Tomohito Ando --- .../launch/motion_velocity_smoother.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml index 78bddc2b81f4f..86f767560fd7c 100644 --- a/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml +++ b/planning/motion_velocity_smoother/launch/motion_velocity_smoother.launch.xml @@ -13,13 +13,13 @@ - + - + From 59c7eda9c8162b88167c689eb4f30f299200f4e4 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 25 Aug 2023 00:31:54 +0900 Subject: [PATCH 09/10] fix(utils): improve monotonic bound generation logic (#4706) Signed-off-by: satoshi-ota --- .../behavior_path_planner/src/utils/utils.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 6da5ff97b9bf8..eb2859ff700af 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1795,8 +1795,22 @@ void makeBoundLongitudinallyMonotonic( continue; } - for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { - set_orientation(ret, j, getPose(path_points.at(i)).orientation); + if (i + 1 == path_points.size()) { + for (size_t j = intersect_idx.get(); j < bound_with_pose.size(); j++) { + if (j + 1 == bound_with_pose.size()) { + const auto yaw = + calcAzimuthAngle(bound_with_pose.at(j - 1).position, bound_with_pose.at(j).position); + set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); + } else { + const auto yaw = + calcAzimuthAngle(bound_with_pose.at(j).position, bound_with_pose.at(j + 1).position); + set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); + } + } + } else { + for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { + set_orientation(ret, j, getPose(path_points.at(i)).orientation); + } } constexpr size_t OVERLAP_CHECK_NUM = 3; From b5e2c13f00ebc5b802c41c847f5c8f5404c9b01b Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 25 Aug 2023 09:05:03 +0900 Subject: [PATCH 10/10] fix(avoidance): avoidance shift line processing bug (#4731) fix(avoidance): safety check chattering Signed-off-by: satoshi-ota --- .../scene_module/avoidance/avoidance_module.hpp | 6 ++++++ .../scene_module/avoidance/avoidance_module.cpp | 15 ++++++++++----- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index fcde58756d4be..7889e91767155 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_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 index b0fda03418069..70999f1504fee 100644 --- 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);