From ca9e2770254109bf00cf35a6498d0de90771370a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 8 May 2023 10:33:15 +0900 Subject: [PATCH 01/21] feat(avoidance): output avoidance target marker that are always shown in rviz (#3582) feat(avoidance): output necessary marker Signed-off-by: satoshi-ota --- .../marker_util/avoidance/debug.hpp | 8 +- .../avoidance/avoidance_module.hpp | 19 ++- .../src/marker_util/avoidance/debug.cpp | 161 ++++++++---------- .../avoidance/avoidance_module.cpp | 49 +++--- 4 files changed, 111 insertions(+), 126 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp index 9b676ecafa2c3..9d39fa32417f5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/avoidance/debug.hpp @@ -58,18 +58,12 @@ MarkerArray createAvoidLineMarkerArray( MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns); -MarkerArray createAvoidableTargetObjectsMarkerArray( - const ObjectDataArray & objects, std::string && ns); - -MarkerArray createUnavoidableTargetObjectsMarkerArray( - const ObjectDataArray & objects, std::string && ns); +MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns); MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); -MarkerArray createUnavoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns); - MarkerArray makeOverhangToRoadShoulderMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns); 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 7eca420e1adb7..3d0fbf72bdf9d 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 @@ -271,12 +271,25 @@ class AvoidanceModule : public SceneModuleInterface // debug mutable DebugData debug_data_; mutable std::shared_ptr debug_msg_ptr_; - void setDebugData( - const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const; - void updateAvoidanceDebugData(std::vector & avoidance_debug_msg_array) const; mutable std::vector debug_avoidance_initializer_for_shift_line_; mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_; + /** + * @brief fill debug markers. + */ + void updateDebugMarker( + const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const; + + /** + * @brief fill information markers that are shown in Rviz by default. + */ + void updateInfoMarker(const AvoidancePlanningData & data) const; + + /** + * @brief fill debug msg that are published as a topic. + */ + void updateAvoidanceDebugData(std::vector & avoidance_debug_msg_array) const; + double getLateralMarginFromVelocity(const double velocity) const; double getRSSLongitudinalDistance( diff --git a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp index b4075932b80ab..fb8322dffeed3 100644 --- a/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp @@ -68,11 +68,34 @@ MarkerArray createObjectsCubeMarkerArray( return msg; } +MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + + for (const auto & object : objects) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + + const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position; + + for (const auto & p : object.envelope_poly.outer()) { + marker.points.push_back(createPoint(p.x(), p.y(), pos.z)); + } + + marker.points.push_back(marker.points.front()); + marker.id = uuidToInt32(object.object.object_id); + msg.markers.push_back(marker); + } + + return msg; +} + MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; - Marker marker = createDefaultMarker( + auto marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); @@ -110,6 +133,40 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st return msg; } +MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + msg.markers.reserve(objects.size() * 4); + + appendMarkerArray( + createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), + createMarkerColor(1.0, 1.0, 0.0, 0.8)), + &msg); + + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + + return msg; +} + +MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + msg.markers.reserve(objects.size() * 4); + + appendMarkerArray( + createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), + createMarkerColor(1.0, 0.0, 0.0, 0.8)), + &msg); + + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); + appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); + + return msg; +} + } // namespace MarkerArray createEgoStatusMarkerArray( @@ -241,7 +298,7 @@ MarkerArray createAvoidLineMarkerArray( for (const auto & sl : shift_lines_local) { // ROS_ERROR("sl: s = (%f, %f), g = (%f, %f)", sl.start.x, sl.start.y, sl.end.x, sl.end.y); - Marker basic_marker = createDefaultMarker( + auto basic_marker = createDefaultMarker( "map", current_time, ns, 0L, Marker::CUBE, createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(r, g, b, 0.9)); basic_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); @@ -337,84 +394,28 @@ MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::st return msg; } -MarkerArray createAvoidableTargetObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns) +MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns) { - MarkerArray msg; - msg.markers.reserve(objects.size() * 3); - - appendMarkerArray( - createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(1.0, 1.0, 0.0, 0.8)), - &msg); - - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); - - { - for (const auto & object : objects) { - const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - - { - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns + "_envelope_polygon", 0L, - Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); - - for (const auto & p : object.envelope_poly.outer()) { - marker.points.push_back(createPoint(p.x(), p.y(), pos.z)); - } - - marker.points.push_back(marker.points.front()); - marker.id = uuidToInt32(object.object.object_id); - msg.markers.push_back(marker); - } + ObjectDataArray avoidable; + ObjectDataArray unavoidable; + for (const auto & o : objects) { + if (o.is_avoidable) { + avoidable.push_back(o); + } else { + unavoidable.push_back(o); } } - return msg; -} - -MarkerArray createUnavoidableTargetObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns) -{ MarkerArray msg; - msg.markers.reserve(objects.size() * 3); + msg.markers.reserve(objects.size() * 4); - appendMarkerArray( - createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), - createMarkerColor(1.0, 0.0, 0.0, 0.8)), - &msg); - - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); - - { - for (const auto & object : objects) { - const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - - { - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns + "_envelope_polygon", 0L, - Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), - createMarkerColor(1.0, 1.0, 1.0, 0.999)); - - for (const auto & p : object.envelope_poly.outer()) { - marker.points.push_back(createPoint(p.x(), p.y(), pos.z)); - } - - marker.points.push_back(marker.points.front()); - marker.id = uuidToInt32(object.object.object_id); - msg.markers.push_back(marker); - } - } - } + appendMarkerArray(avoidableObjectsMarkerArray(avoidable, "avoidable_" + ns), &msg); + appendMarkerArray(unAvoidableObjectsMarkerArray(unavoidable, "unavoidable_" + ns), &msg); return msg; } -MarkerArray createOtherObjectsMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, const std::string & ns) +MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns) { using behavior_path_planner::utils::convertToSnakeCase; @@ -451,24 +452,10 @@ MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std: objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), createMarkerColor(0.0, 0.0, 1.0, 0.8)); } -MarkerArray createUnavoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) -{ - MarkerArray msg; - - appendMarkerArray( - createObjectsCubeMarkerArray( - objects, ns + "_cube", createMarkerScale(3.2, 1.7, 2.0), - createMarkerColor(1.0, 0.0, 1.0, 0.9)), - &msg); - appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); - - return msg; -} - MarkerArray makeOverhangToRoadShoulderMarkerArray( - const behavior_path_planner::ObjectDataArray & objects, std::string && ns) + const ObjectDataArray & objects, std::string && ns) { - Marker marker = createDefaultMarker( + auto marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 1.0)); @@ -495,7 +482,7 @@ MarkerArray createOverhangFurthestLineStringMarkerArray( for (const auto & linestring : linestrings) { const auto id = static_cast(linestring.id()); - Marker marker = createDefaultMarker( + auto marker = createDefaultMarker( "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.4, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); 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 4420525bfa14e..a0889186d13c5 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 @@ -125,9 +125,8 @@ bool AvoidanceModule::isExecutionRequested() const const auto avoid_data = avoidance_data_; #endif - if (parameters_->publish_debug_marker) { - setDebugData(avoid_data, path_shifter_, debug_data_); - } + updateInfoMarker(avoid_data); + updateDebugMarker(avoid_data, path_shifter_, debug_data_); #ifndef USE_OLD_ARCHITECTURE // there is object that should be avoid. return true. @@ -2800,11 +2799,8 @@ BehaviorModuleOutput AvoidanceModule::plan() updateEgoBehavior(data, avoidance_path); } - if (parameters_->publish_debug_marker) { - setDebugData(avoidance_data_, path_shifter_, debug_data_); - } else { - debug_marker_.markers.clear(); - } + updateInfoMarker(avoidance_data_); + updateDebugMarker(avoidance_data_, path_shifter_, debug_data_); output.path = std::make_shared(avoidance_path.path); output.reference_path = getPreviousModuleOutput().reference_path; @@ -3242,7 +3238,16 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con return turn_signal_info; } -void AvoidanceModule::setDebugData( +void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const +{ + using marker_utils::avoidance_marker::createTargetObjectsMarkerArray; + + info_marker_.markers.clear(); + appendMarkerArray( + createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_); +} + +void AvoidanceModule::updateDebugMarker( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { using marker_utils::createLaneletsAreaMarkerArray; @@ -3251,25 +3256,25 @@ void AvoidanceModule::setDebugData( using marker_utils::createPoseMarkerArray; using marker_utils::createShiftLengthMarkerArray; using marker_utils::createShiftLineMarkerArray; - using marker_utils::avoidance_marker::createAvoidableTargetObjectsMarkerArray; using marker_utils::avoidance_marker::createAvoidLineMarkerArray; using marker_utils::avoidance_marker::createEgoStatusMarkerArray; using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; using marker_utils::avoidance_marker::createPredictedVehiclePositions; using marker_utils::avoidance_marker::createSafetyCheckMarkerArray; - using marker_utils::avoidance_marker::createUnavoidableObjectsMarkerArray; - using marker_utils::avoidance_marker::createUnavoidableTargetObjectsMarkerArray; using marker_utils::avoidance_marker::createUnsafeObjectsMarkerArray; using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; using tier4_autoware_utils::appendMarkerArray; debug_marker_.markers.clear(); + + if (!parameters_->publish_debug_marker) { + return; + } + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; + const auto add = [this](const MarkerArray & added) { appendMarkerArray(added, &debug_marker_); }; const auto addAvoidLine = [&](const AvoidLineArray & al_arr, const auto & ns, auto r, auto g, auto b, double w = 0.1) { @@ -3293,22 +3298,8 @@ void AvoidanceModule::setDebugData( add(createSafetyCheckMarkerArray(data.state, getEgoPose(), debug)); - std::vector avoidable_target_objects; - std::vector unavoidable_target_objects; - for (const auto & object : data.target_objects) { - if (object.is_avoidable) { - avoidable_target_objects.push_back(object); - } else { - unavoidable_target_objects.push_back(object); - } - } - add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); add(createLaneletsAreaMarkerArray(*debug.expanded_lanelets, "expanded_lanelet", 0.8, 0.8, 0.0)); - add( - createAvoidableTargetObjectsMarkerArray(avoidable_target_objects, "avoidable_target_objects")); - add(createUnavoidableTargetObjectsMarkerArray( - unavoidable_target_objects, "unavoidable_target_objects")); add(createOtherObjectsMarkerArray( data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD)); From 161e23df27211d4c44da42ddadc4ad8be7a1ce6e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 8 May 2023 10:33:46 +0900 Subject: [PATCH 02/21] fix(avoidance): fix visualization bug (#3586) fix(avoidance): ignore objects whose shift line length is less than threshold Signed-off-by: satoshi-ota --- .../utils/avoidance/utils.hpp | 2 ++ .../avoidance/avoidance_module.cpp | 1 + .../src/utils/avoidance/utils.cpp | 35 +++++++++++++++++++ 3 files changed, 38 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 7873e39dc8de9..d5ab62ddc23e5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -34,6 +34,8 @@ bool isTargetObjectType( double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin); +bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length); + bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length); size_t findPathIndexFromArclength( 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 a0889186d13c5..d028fc9fc3800 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 @@ -3313,6 +3313,7 @@ void AvoidanceModule::updateDebugMarker( add(createOtherObjectsMarkerArray(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT)); add(createOtherObjectsMarkerArray(data.other_objects, std::string("MovingObject"))); add(createOtherObjectsMarkerArray(data.other_objects, std::string("OutOfTargetArea"))); + add(createOtherObjectsMarkerArray(data.other_objects, std::string("NotNeedAvoidance"))); add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); add(createOverhangFurthestLineStringMarkerArray( diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 7e05946d7d29f..893d15f9bb5ea 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -96,6 +96,32 @@ double calcShiftLength( return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0; } +bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length) +{ + /** + * ^ + * | + * --+----x-------------------------------x---> + * | x x + * | ==obj== + */ + if (is_object_on_right && shift_length < 0.0) { + return false; + } + + /** + * ^ ==obj== + * | x x + * --+----x-------------------------------x---> + * | + */ + if (!is_object_on_right && shift_length > 0.0) { + return false; + } + + return true; +} + bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length) { return (is_object_on_right == std::signbit(shift_length)); @@ -683,6 +709,15 @@ void filterTargetObjects( return std::min(max_allowable_lateral_distance, max_avoid_margin); }(); + if (!!avoid_margin) { + const auto shift_length = calcShiftLength(isOnRight(o), o.overhang_dist, avoid_margin.get()); + if (!isShiftNecessary(isOnRight(o), shift_length)) { + o.reason = "NotNeedAvoidance"; + data.other_objects.push_back(o); + continue; + } + } + // force avoidance for stopped vehicle { const auto to_traffic_light = From db8e81e92e14ee417dd3015c87de04a80f77580f Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Mon, 8 May 2023 10:37:41 +0900 Subject: [PATCH 03/21] docs(.pages): ignore top-level documents for better visibility (#3625) Signed-off-by: Kenji Miyake --- .pages | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .pages diff --git a/.pages b/.pages new file mode 100644 index 0000000000000..7ee8aa0513a53 --- /dev/null +++ b/.pages @@ -0,0 +1,2 @@ +nav: + - ... | regex=^(?!.*(README.md|CONTRIBUTING.md|CODE_OF_CONDUCT.md|DISCLAIMER.md)).*$ From de8afd76c123f5698c7fd8011bb4fe2d426f5eb1 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 8 May 2023 10:50:30 +0900 Subject: [PATCH 04/21] refactor(avoidance_by_lane_change): reuse lane change class (#3577) * refactor(avoidance_by_lane_change): reuse lane change class Signed-off-by: Muhammad Zulfaqar Azmi * fix wrong direction Signed-off-by: Muhammad Zulfaqar * fix conflict Signed-off-by: Muhammad Zulfaqar * fix rtc paramters Signed-off-by: Muhammad Zulfaqar * Revert "fix rtc paramters" This reverts commit 16be270af3225cbe9e826a89b048a6dd044e98b9. * fix rtc Signed-off-by: Muhammad Zulfaqar Azmi * fix pre-commit Signed-off-by: Muhammad Zulfaqar Azmi * fix pre-commit and move updateRTCStatus to cpp file Signed-off-by: Muhammad Zulfaqar Azmi * COMPILE_WITH_OLD_ARCHITECTURE=TRUE Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar --- planning/behavior_path_planner/CMakeLists.txt | 3 +- .../behavior_path_planner_node.hpp | 2 - .../lane_change/avoidance_by_lane_change.hpp | 62 +++ .../scene_module/lane_change/base_class.hpp | 16 +- .../scene_module/lane_change/interface.hpp | 30 +- .../scene_module/lane_change/manager.hpp | 19 +- .../scene_module/lane_change/normal.hpp | 5 +- .../utils/avoidance_by_lc/module_data.hpp | 45 -- .../lane_change/lane_change_module_data.hpp | 23 +- .../utils/lane_change/utils.hpp | 16 - .../src/behavior_path_planner_node.cpp | 8 +- .../scene_module/avoidance_by_lc/module.cpp | 387 ------------------ .../lane_change/avoidance_by_lane_change.cpp | 224 ++++++++++ .../scene_module/lane_change/interface.cpp | 70 ++++ .../src/scene_module/lane_change/manager.cpp | 30 ++ .../src/scene_module/lane_change/normal.cpp | 40 +- .../src/utils/lane_change/utils.cpp | 230 ----------- 17 files changed, 487 insertions(+), 723 deletions(-) create mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp create mode 100644 planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 8cad60065540b..0de6dc33582ea 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -14,7 +14,6 @@ set(common_src src/behavior_path_planner_node.cpp src/scene_module/scene_module_visitor.cpp src/scene_module/avoidance/avoidance_module.cpp - src/scene_module/avoidance_by_lc/module.cpp src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp src/scene_module/pull_out/pull_out_module.cpp src/scene_module/goal_planner/goal_planner_module.cpp @@ -22,6 +21,7 @@ set(common_src src/scene_module/lane_change/interface.cpp src/scene_module/lane_change/normal.cpp src/scene_module/lane_change/external_request.cpp + src/scene_module/lane_change/avoidance_by_lane_change.cpp src/turn_signal_decider.cpp src/utils/utils.cpp src/utils/path_utils.cpp @@ -66,7 +66,6 @@ else() ament_auto_add_library(behavior_path_planner_node SHARED src/planner_manager.cpp src/scene_module/avoidance/manager.cpp - src/scene_module/avoidance_by_lc/manager.cpp src/scene_module/dynamic_avoidance/manager.cpp src/scene_module/pull_out/manager.cpp src/scene_module/goal_planner/manager.cpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 037c470c23dc0..2c1ef1038554e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -29,7 +29,6 @@ #else #include "behavior_path_planner/planner_manager.hpp" #include "behavior_path_planner/scene_module/avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp" #include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" #include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/scene_module/lane_change/manager.hpp" @@ -40,7 +39,6 @@ #include "behavior_path_planner/steering_factor_interface.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp new file mode 100644 index 0000000000000..e58a8c07850cf --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp @@ -0,0 +1,62 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ +#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ + +#include "behavior_path_planner/scene_module/lane_change/normal.hpp" + +#include +#include + +namespace behavior_path_planner +{ +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::LaneChangeDebugMsg; +using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; +using AvoidanceDebugData = DebugData; + +class AvoidanceByLaneChange : public NormalLaneChange +{ +public: + AvoidanceByLaneChange( + const std::shared_ptr & parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters); + + void updateSpecialData() override; + + std::pair getSafePath(LaneChangePath & safe_path) const override; + +private: + std::shared_ptr avoidance_parameters_; + std::shared_ptr avoidance_by_lane_change_parameters_; + + AvoidancePlanningData calcAvoidancePlanningData(AvoidanceDebugData & debug) const; + AvoidancePlanningData avoidance_data_; + mutable AvoidanceDebugData avoidance_debug_data_; + + ObjectDataArray registered_objects_; + mutable ObjectDataArray stopped_objects_; + + ObjectData createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const; + + void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__AVOIDANCE_BY_LANE_CHANGE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index de5955f123940..011431f5017b3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -55,7 +55,7 @@ class LaneChangeBase LaneChangeBase( std::shared_ptr parameters, LaneChangeModuleType type, Direction direction) - : parameters_{std::move(parameters)}, direction_{direction}, type_{type} + : lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type} { prev_module_reference_path_ = std::make_shared(); prev_module_path_ = std::make_shared(); @@ -107,6 +107,8 @@ class LaneChangeBase } } + virtual void updateSpecialData() {} + const LaneChangeStatus & getLaneChangeStatus() const { return status_; } LaneChangePath getLaneChangePath() const @@ -120,7 +122,7 @@ class LaneChangeBase bool isAbortState() const { - if (!parameters_->enable_abort_lane_change) { + if (!lane_change_parameters_->enable_abort_lane_change) { return false; } @@ -167,7 +169,7 @@ class LaneChangeBase Direction getDirection() const { - if (direction_ == Direction::NONE) { + if (direction_ == Direction::NONE && !status_.lane_change_path.path.points.empty()) { const auto lateral_shift = utils::lane_change::getLateralShift(status_.lane_change_path); return lateral_shift > 0.0 ? Direction::LEFT : Direction::RIGHT; } @@ -187,7 +189,8 @@ class LaneChangeBase virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, LaneChangePaths * candidate_paths) const = 0; + const lanelet::ConstLanelets & target_lanelets, Direction direction, + LaneChangePaths * candidate_paths) const = 0; virtual std::vector getDrivableLanes() const = 0; @@ -197,7 +200,8 @@ class LaneChangeBase virtual bool isValidPath(const PathWithLaneId & path) const = 0; - lanelet::ConstLanelets getLaneChangeLanes(const lanelet::ConstLanelets & current_lanes) const; + virtual lanelet::ConstLanelets getLaneChangeLanes( + const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0; bool isNearEndOfLane() const { @@ -222,7 +226,7 @@ class LaneChangeBase LaneChangeStates current_lane_change_state_{}; - std::shared_ptr parameters_{}; + std::shared_ptr lane_change_parameters_{}; std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; std::shared_ptr prev_module_reference_path_{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index b098eecdec231..653c95b060163 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ #include "behavior_path_planner/marker_util/lane_change/debug.hpp" +#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/scene_module/lane_change/base_class.hpp" #include "behavior_path_planner/scene_module/lane_change/external_request.hpp" #include "behavior_path_planner/scene_module/lane_change/normal.hpp" @@ -74,6 +75,8 @@ class LaneChangeInterface : public SceneModuleInterface ModuleStatus updateState() override; + void updateData() override; + BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -113,6 +116,22 @@ class LaneChangeInterface : public SceneModuleInterface bool is_abort_approval_requested_{false}; }; +class AvoidanceByLaneChangeInterface : public LaneChangeInterface +{ +public: + AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map > & rtc_interface_ptr_map); + + ModuleStatus updateState() override; + +protected: + void updateRTCStatus(const double start_distance, const double finish_distance) override; +}; + class LaneChangeBTInterface : public LaneChangeInterface { public: @@ -150,16 +169,7 @@ class LaneChangeBTModule : public LaneChangeBTInterface const std::shared_ptr & parameters); protected: - void updateRTCStatus(const double start_distance, const double finish_distance) override - { - const auto direction = std::invoke([&]() -> std::string { - const auto dir = module_type_->getDirection(); - return (dir == Direction::LEFT) ? "left" : "right"; - }); - - rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( - uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); - } + void updateRTCStatus(const double start_distance, const double finish_distance) override; }; class ExternalRequestLaneChangeLeftBTModule : public LaneChangeBTInterface diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index 39452d6367fae..ebee8f3d3a07a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -42,7 +42,7 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; -private: +protected: std::shared_ptr parameters_; std::vector> registered_modules_; @@ -52,6 +52,23 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface LaneChangeModuleType type_; }; +class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager +{ +public: + AvoidanceByLaneChangeModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + std::shared_ptr parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters); + + std::shared_ptr createNewSceneModuleInstance() override; + + // void updateModuleParams(const std::vector & parameters) override; + +private: + std::shared_ptr avoidance_parameters_; + std::shared_ptr avoidance_by_lane_change_parameters_; +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 11a7574bb075e..c4744b0ccbd38 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -68,7 +68,8 @@ class NormalLaneChange : public LaneChangeBase protected: lanelet::ConstLanelets getCurrentLanes() const override; - lanelet::ConstLanelets getLaneChangeLanes(const lanelet::ConstLanelets & current_lanes) const; + lanelet::ConstLanelets getLaneChangeLanes( + const lanelet::ConstLanelets & current_lanes, Direction direction) const override; int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; @@ -79,7 +80,7 @@ class NormalLaneChange : public LaneChangeBase bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, + const lanelet::ConstLanelets & target_lanelets, Direction direction, LaneChangePaths * candidate_paths) const override; std::vector getDrivableLanes() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp deleted file mode 100644 index e556c56eac8d7..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance_by_lc/module_data.hpp +++ /dev/null @@ -1,45 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_ - -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" - -#include - -namespace behavior_path_planner -{ - -struct AvoidanceByLCParameters -{ - // avoidance - std::shared_ptr avoidance; - - // lane change - std::shared_ptr lane_change; - - // execute if the target object number is larger than this param. - size_t execute_object_num{1}; - - // execute only when the target object longitudinal distance is larger than this param. - double execute_object_longitudinal_margin{0.0}; - - // execute only when lane change end point is before the object. - bool execute_only_when_lane_change_finish_before_object{false}; -}; - -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE_BY_LC__MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index b19e7469c0600..e81f8821b25ed 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -14,10 +14,12 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ +#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "lanelet2_core/geometry/Lanelet.h" #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" +#include #include #include @@ -79,7 +81,26 @@ struct LaneChangeTargetObjectIndices std::vector other_lane{}; }; -enum class LaneChangeModuleType { NORMAL = 0, EXTERNAL_REQUEST }; +enum class LaneChangeModuleType { + NORMAL = 0, + EXTERNAL_REQUEST, + AVOIDANCE_BY_LANE_CHANGE, +}; + +struct AvoidanceByLCParameters +{ + std::shared_ptr avoidance{}; + std::shared_ptr lane_change{}; + + // execute if the target object number is larger than this param. + size_t execute_object_num{1}; + + // execute only when the target object longitudinal distance is larger than this param. + double execute_object_longitudinal_margin{0.0}; + + // execute only when lane change end point is before the object. + bool execute_only_when_lane_change_finish_before_object{false}; +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 43514b011c6fb..8702e7a8747a0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -79,22 +79,6 @@ std::optional constructCandidatePath( const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & lane_change_param); -bool getLaneChangePaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data); - -bool getLaneChangePaths( - const PathWithLaneId & original_path, const RouteHandler & route_handler, - const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, const Direction direction, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data); - bool isLaneChangePathSafe( const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose, diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index e58c71f00fc62..bb491086992f0 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -286,8 +286,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod } if (p.config_avoidance_by_lc.enable_module) { - auto manager = std::make_shared( - this, "avoidance_by_lane_change", p.config_avoidance_by_lc, avoidance_by_lc_param_ptr_); + auto manager = std::make_shared( + this, "avoidance_by_lane_change", p.config_avoidance_by_lc, lane_change_param_ptr_, + avoidance_param_ptr_, avoidance_by_lc_param_ptr_); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( "avoidance_by_lane_change", @@ -491,6 +492,9 @@ AvoidanceByLCParameters BehaviorPathPlannerNode::getAvoidanceByLCParam( declare_parameter(ns + "execute_only_when_lane_change_finish_before_object"); } + if (p.execute_object_num < 1) { + RCLCPP_WARN_STREAM(get_logger(), "execute_object_num cannot be lesser than 1."); + } return p; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp index e3f032d3d0be3..c76034d03cecb 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp @@ -55,406 +55,21 @@ AvoidanceByLCModule::AvoidanceByLCModule( : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} { } - -void AvoidanceByLCModule::processOnEntry() -{ -#ifndef USE_OLD_ARCHITECTURE - waitApproval(); -#endif - current_lane_change_state_ = LaneChangeStates::Normal; - updateLaneChangeStatus(); -} - -void AvoidanceByLCModule::processOnExit() -{ - resetParameters(); -} - -bool AvoidanceByLCModule::isExecutionRequested() const -{ - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - - if (!found_valid_path) { - return false; - } - - const auto object_num = avoidance_data_.target_objects.size(); - if (parameters_->execute_object_num > object_num) { - return false; - } - - const auto to_front_object_distance = avoidance_data_.target_objects.front().longitudinal; - if (parameters_->execute_object_longitudinal_margin > to_front_object_distance) { - return false; - } - - const auto to_lane_change_end_distance = calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.end.position); - const auto lane_change_finish_before_object = - to_front_object_distance > to_lane_change_end_distance; - if ( - !lane_change_finish_before_object && - parameters_->execute_only_when_lane_change_finish_before_object) { - return false; - } - - return true; -} - -bool AvoidanceByLCModule::isExecutionReady() const -{ - if (current_state_ == ModuleStatus::RUNNING) { - return true; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); - - return found_safe_path; -} - -void AvoidanceByLCModule::updateData() -{ - debug_data_ = DebugData(); - avoidance_data_ = calcAvoidancePlanningData(debug_data_); - - utils::avoidance::updateRegisteredObject( - registered_objects_, avoidance_data_.target_objects, parameters_->avoidance); - utils::avoidance::compensateDetectionLost( - registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); - - std::sort( - avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), - [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); -} - -AvoidancePlanningData AvoidanceByLCModule::calcAvoidancePlanningData(DebugData & debug) const -{ - AvoidancePlanningData data; - - // reference pose - data.reference_pose = getEgoPose(); - - data.reference_path_rough = *getPreviousModuleOutput().path; - - data.reference_path = utils::resamplePathWithSpline( - data.reference_path_rough, parameters_->avoidance->resample_interval_for_planning); - - // lanelet info -#ifdef USE_OLD_ARCHITECTURE - data.current_lanelets = utils::getCurrentLanes(planner_data_); -#else - data.current_lanelets = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - - // target objects for avoidance - fillAvoidanceTargetObjects(data, debug); - - return data; -} - -void AvoidanceByLCModule::fillAvoidanceTargetObjects( - AvoidancePlanningData & data, DebugData & debug) const -{ - const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( - data.current_lanelets, parameters_->avoidance->detection_area_left_expand_dist, - parameters_->avoidance->detection_area_right_expand_dist * (-1.0)); - - const auto [object_within_target_lane, object_outside_target_lane] = - utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); - - for (const auto & object : object_outside_target_lane.objects) { - ObjectData other_object; - other_object.object = object; - other_object.reason = "OutOfTargetArea"; - data.other_objects.push_back(other_object); - } - - ObjectDataArray objects; - for (const auto & object : object_within_target_lane.objects) { - objects.push_back(createObjectData(data, object)); - } - - utils::avoidance::filterTargetObjects( - objects, data, debug, planner_data_, parameters_->avoidance); -} - -ObjectData AvoidanceByLCModule::createObjectData( - const AvoidancePlanningData & data, const PredictedObject & object) const -{ - using boost::geometry::return_centroid; - - const auto & path_points = data.reference_path.points; - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = findNearestIndex(path_points, object_pose.position); - const auto object_closest_pose = path_points.at(object_closest_index).point.pose; - - ObjectData object_data{}; - - object_data.object = object; - - // Calc envelop polygon. - utils::avoidance::fillObjectEnvelopePolygon( - object_data, registered_objects_, object_closest_pose, parameters_->avoidance); - - // calc object centroid. - object_data.centroid = return_centroid(object_data.envelope_poly); - - // Calc moving time. - utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_->avoidance); - - // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); - - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, object_closest_pose, object_data.overhang_pose.position); - - // Check whether the the ego should avoid the object. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto safety_margin = - 0.5 * vehicle_width + parameters_->avoidance->lateral_passable_safety_buffer; - object_data.avoid_required = - (utils::avoidance::isOnRight(object_data) && - std::abs(object_data.overhang_dist) < safety_margin) || - (!utils::avoidance::isOnRight(object_data) && object_data.overhang_dist < safety_margin); - - return object_data; -} - -ModuleStatus AvoidanceByLCModule::updateState() -{ - RCLCPP_DEBUG(getLogger(), "AVOIDANCE_BY_LC updateState"); - if (!isValidPath()) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; - } - - if (isWaitingApproval()) { - const auto object_num = avoidance_data_.target_objects.size(); - if (parameters_->execute_object_num > object_num) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; - } - - const auto to_front_object_distance = avoidance_data_.target_objects.front().longitudinal; - if (parameters_->execute_object_longitudinal_margin > to_front_object_distance) { - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - - const auto to_lane_change_end_distance = calcSignedArcLength( - status_.lane_change_path.path.points, getEgoPose().position, - status_.lane_change_path.shift_line.end.position); - const auto lane_change_finish_before_object = - to_front_object_distance > to_lane_change_end_distance; - if ( - !lane_change_finish_before_object && - parameters_->execute_only_when_lane_change_finish_before_object) { - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - } - - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - if (isAbortState() && !is_within_current_lane) { - current_state_ = ModuleStatus::RUNNING; - return current_state_; - } - - if (isAbortConditionSatisfied()) { - if ((isNearEndOfLane() && isCurrentVelocityLow()) || !is_within_current_lane) { - current_state_ = ModuleStatus::RUNNING; - return current_state_; - } - - current_state_ = ModuleStatus::FAILURE; - return current_state_; - } - - if (hasFinishedLaneChange()) { - current_state_ = ModuleStatus::SUCCESS; - return current_state_; - } - current_state_ = ModuleStatus::RUNNING; - return current_state_; -} - -BehaviorModuleOutput AvoidanceByLCModule::plan() -{ - resetPathCandidate(); - resetPathReference(); - is_activated_ = isActivated(); - - PathWithLaneId path = status_.lane_change_path.path; - if (!isValidPath(path)) { - status_.is_valid_path = false; - return BehaviorModuleOutput{}; - } else { - status_.is_valid_path = true; - } - - if ((is_abort_condition_satisfied_ && isNearEndOfLane() && isCurrentVelocityLow())) { - const auto stop_point = utils::insertStopPoint(0.1, path); - } - - if (isAbortState()) { - resetPathIfAbort(); - if (is_activated_) { - path = abort_path_->path; - } - } - - generateExtendedDrivableArea(path); - - BehaviorModuleOutput output; - output.path = std::make_shared(path); -#ifdef USE_OLD_ARCHITECTURE - path_reference_ = getPreviousModuleOutput().reference_path; - prev_approved_path_ = path; -#else - const auto reference_path = - utils::getCenterLinePathFromRootLanelet(status_.lane_change_lanes.front(), planner_data_); - output.reference_path = std::make_shared(reference_path); - path_reference_ = std::make_shared(reference_path); - prev_approved_path_ = *getPreviousModuleOutput().path; -#endif - updateOutputTurnSignal(output); - - updateSteeringFactorPtr(output); - clearWaitingApproval(); - - return output; -} - -void AvoidanceByLCModule::resetPathIfAbort() -{ - if (!is_abort_approval_requested_) { -#ifdef USE_OLD_ARCHITECTURE - const auto lateral_shift = utils::lane_change::getLateralShift(*abort_path_); - if (lateral_shift > 0.0) { - removePreviousRTCStatusRight(); - uuid_map_.at("right") = generateUUID(); - } else if (lateral_shift < 0.0) { - removePreviousRTCStatusLeft(); - uuid_map_.at("left") = generateUUID(); - } -#else - removeRTCStatus(); -#endif - RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); - is_abort_approval_requested_ = true; - is_abort_path_approved_ = false; - return; - } - - if (isActivated()) { - RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is true. set is_abort_path_approved to true."); - is_abort_path_approved_ = true; - clearWaitingApproval(); - } else { - RCLCPP_DEBUG(getLogger(), "[abort] isActivated() is False."); - is_abort_path_approved_ = false; - waitApproval(); - } -} - -CandidateOutput AvoidanceByLCModule::planCandidate() const -{ - CandidateOutput output; - - LaneChangePath selected_path; - // Get lane change lanes -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); - - if (lane_change_lanes.empty()) { - return output; - } - -#ifdef USE_OLD_ARCHITECTURE - [[maybe_unused]] const auto [found_valid_path, found_safe_path] = - getSafePath(lane_change_lanes, check_distance_, selected_path); -#else - selected_path = status_.lane_change_path; -#endif - - selected_path.path.header = planner_data_->route_handler->getRouteHeader(); - - if (isAbortState()) { - selected_path = *abort_path_; - } - - if (selected_path.path.points.empty()) { - return output; - } - - output.path_candidate = selected_path.path; - output.lateral_shift = utils::lane_change::getLateralShift(selected_path); - output.start_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.start.position); - output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( - selected_path.path.points, getEgoPose().position, selected_path.shift_line.end.position); - - updateSteeringFactorPtr(output, selected_path); - return output; -} - BehaviorModuleOutput AvoidanceByLCModule::planWaitingApproval() { -#ifdef USE_OLD_ARCHITECTURE - const auto is_within_current_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters); - if (is_within_current_lane) { - prev_approved_path_ = getReferencePath(); - } -#else prev_approved_path_ = *getPreviousModuleOutput().path; -#endif BehaviorModuleOutput out; out.path = std::make_shared(prev_approved_path_); out.reference_path = getPreviousModuleOutput().reference_path; out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; // for new architecture -#ifndef USE_OLD_ARCHITECTURE const auto current_lanes = utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, drivable_lanes); out.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes); -#endif if (!avoidance_data_.target_objects.empty()) { const auto to_front_object_distance = avoidance_data_.target_objects.front().longitudinal; @@ -467,9 +82,7 @@ BehaviorModuleOutput AvoidanceByLCModule::planWaitingApproval() getEgoPosition(), to_front_object_distance - lane_change_buffer, 0.0, *out.path, stop_pose_); } -#ifndef USE_OLD_ARCHITECTURE updateLaneChangeStatus(); -#endif const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp new file mode 100644 index 0000000000000..17fc83370d750 --- /dev/null +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -0,0 +1,224 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" + +#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_utils.hpp" + +#include + +#include + +namespace behavior_path_planner +{ +AvoidanceByLaneChange::AvoidanceByLaneChange( + const std::shared_ptr & parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters) +: NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE), + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_by_lane_change_parameters_(std::move(avoidance_by_lane_change_parameters)) +{ +} + +std::pair AvoidanceByLaneChange::getSafePath(LaneChangePath & safe_path) const +{ + const auto & avoidance_objects = avoidance_data_.target_objects; + const auto execute_object_num = avoidance_by_lane_change_parameters_->execute_object_num; + + if (avoidance_objects.size() < execute_object_num) { + return {false, false}; + } + + const auto current_lanes = getCurrentLanes(); + + if (current_lanes.empty()) { + return {false, false}; + } + + const auto & o_front = avoidance_objects.front(); + + // check distance from ego to o_front vs acceptable longitudinal margin + const auto execute_object_longitudinal_margin = + avoidance_by_lane_change_parameters_->execute_object_longitudinal_margin; + if (execute_object_longitudinal_margin > o_front.longitudinal) { + return {false, false}; + } + + const auto direction = utils::avoidance::isOnRight(o_front) ? Direction::LEFT : Direction::RIGHT; + const auto target_lanes = getLaneChangeLanes(current_lanes, direction); + + if (target_lanes.empty()) { + return {false, false}; + } + + // find candidate paths + LaneChangePaths valid_paths{}; + const auto found_safe_path = + getLaneChangePaths(current_lanes, target_lanes, direction, &valid_paths); + + if (valid_paths.empty()) { + return {false, false}; + } + + if (found_safe_path) { + safe_path = valid_paths.back(); + } else { + // force candidate + safe_path = valid_paths.front(); + } + + const auto to_lane_change_end_distance = motion_utils::calcSignedArcLength( + safe_path.path.points, getEgoPose().position, safe_path.shift_line.end.position); + const auto lane_change_finish_before_object = o_front.longitudinal > to_lane_change_end_distance; + const auto execute_only_when_lane_change_finish_before_object = + avoidance_by_lane_change_parameters_->execute_only_when_lane_change_finish_before_object; + if (!lane_change_finish_before_object && execute_only_when_lane_change_finish_before_object) { + return {false, found_safe_path}; + } + return {true, found_safe_path}; +} + +void AvoidanceByLaneChange::updateSpecialData() +{ + avoidance_debug_data_ = DebugData(); + avoidance_data_ = calcAvoidancePlanningData(avoidance_debug_data_); + + utils::avoidance::updateRegisteredObject( + registered_objects_, avoidance_data_.target_objects, + avoidance_by_lane_change_parameters_->avoidance); + utils::avoidance::compensateDetectionLost( + registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + + std::sort( + avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), + [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); +} + +AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( + AvoidanceDebugData & debug) const +{ + AvoidancePlanningData data; + + // reference pose + data.reference_pose = getEgoPose(); + + data.reference_path_rough = *prev_module_path_; + + const auto resample_interval = + avoidance_by_lane_change_parameters_->avoidance->resample_interval_for_planning; + data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval); + + data.current_lanelets = getCurrentLanes(); + + // get related objects from dynamic_objects, and then separates them as target objects and non + // target objects + fillAvoidanceTargetObjects(data, debug); + + return data; +} + +void AvoidanceByLaneChange::fillAvoidanceTargetObjects( + AvoidancePlanningData & data, DebugData & debug) const +{ + const auto left_expand_dist = avoidance_parameters_->detection_area_left_expand_dist; + const auto right_expand_dist = avoidance_parameters_->detection_area_right_expand_dist; + + const auto expanded_lanelets = lanelet::utils::getExpandedLanelets( + data.current_lanelets, left_expand_dist, right_expand_dist * (-1.0)); + + const auto [object_within_target_lane, object_outside_target_lane] = + utils::separateObjectsByLanelets(*planner_data_->dynamic_object, expanded_lanelets); + + // Assume that the maximum allocation for data.other object is the sum of + // objects_within_target_lane and object_outside_target_lane. The maximum allocation for + // data.target_objects is equal to object_within_target_lane + { + const auto other_objects_size = + object_within_target_lane.objects.size() + object_outside_target_lane.objects.size(); + data.other_objects.reserve(other_objects_size); + data.target_objects.reserve(object_within_target_lane.objects.size()); + } + + { + const auto & objects = object_outside_target_lane.objects; + std::transform( + objects.cbegin(), objects.cend(), std::back_inserter(data.other_objects), + [](const auto & object) { + ObjectData other_object; + other_object.object = object; + other_object.reason = "OutOfTargetArea"; + return other_object; + }); + } + + ObjectDataArray target_lane_objects; + target_lane_objects.reserve(object_within_target_lane.objects.size()); + { + const auto & objects = object_within_target_lane.objects; + std::transform( + objects.cbegin(), objects.cend(), std::back_inserter(target_lane_objects), + [&](const auto & object) { return createObjectData(data, object); }); + } + + utils::avoidance::filterTargetObjects( + target_lane_objects, data, debug, planner_data_, avoidance_parameters_); +} + +ObjectData AvoidanceByLaneChange::createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const +{ + using boost::geometry::return_centroid; + + const auto & path_points = data.reference_path.points; + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = + motion_utils::findNearestIndex(path_points, object_pose.position); + const auto object_closest_pose = path_points.at(object_closest_index).point.pose; + + ObjectData object_data{}; + + object_data.object = object; + + // Calc envelop polygon. + utils::avoidance::fillObjectEnvelopePolygon( + object_data, registered_objects_, object_closest_pose, avoidance_parameters_); + + // calc object centroid. + object_data.centroid = return_centroid(object_data.envelope_poly); + + // Calc moving time. + utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, avoidance_parameters_); + + // Calc lateral deviation from path to target object. + object_data.lateral = + tier4_autoware_utils::calcLateralDeviation(object_closest_pose, object_pose.position); + + // Find the footprint point closest to the path, set to object_data.overhang_distance. + object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( + object_data, object_closest_pose, object_data.overhang_pose.position); + + // Check whether the the ego should avoid the object. + const auto vehicle_width = planner_data_->parameters.vehicle_width; + const auto safety_margin = + 0.5 * vehicle_width + avoidance_parameters_->lateral_passable_safety_buffer; + object_data.avoid_required = + (utils::avoidance::isOnRight(object_data) && + std::abs(object_data.overhang_dist) < safety_margin) || + (!utils::avoidance::isOnRight(object_data) && object_data.overhang_dist < safety_margin); + + return object_data; +} +} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index f222b8f3f5519..9889a94648942 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -110,6 +110,13 @@ ModuleStatus LaneChangeInterface::updateState() return current_state_; } +void LaneChangeInterface::updateData() +{ + module_type_->setPreviousModulePaths( + getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->updateSpecialData(); +} + BehaviorModuleOutput LaneChangeInterface::plan() { resetPathCandidate(); @@ -310,6 +317,57 @@ void SceneModuleVisitor::visitLaneChangeInterface(const LaneChangeInterface * in lane_change_visitor_ = interface->get_debug_msg_array(); } +AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map > & rtc_interface_ptr_map) +: LaneChangeInterface{ + name, node, parameters, rtc_interface_ptr_map, + std::make_unique( + parameters, avoidance_parameters, avoidance_by_lane_change_parameters)} +{ +} + +ModuleStatus AvoidanceByLaneChangeInterface::updateState() +{ + if (!module_type_->isValidPath()) { + current_state_ = ModuleStatus::FAILURE; + return current_state_; + } + + if (module_type_->isAbortState()) { + current_state_ = ModuleStatus::RUNNING; + return current_state_; + } + + if (module_type_->isCancelConditionSatisfied()) { + current_state_ = ModuleStatus::FAILURE; + return current_state_; + } + + if (module_type_->hasFinishedLaneChange()) { + current_state_ = ModuleStatus::SUCCESS; + return current_state_; + } + + current_state_ = ModuleStatus::RUNNING; + return current_state_; +} + +void AvoidanceByLaneChangeInterface::updateRTCStatus( + const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} + LaneChangeBTInterface::LaneChangeBTInterface( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, @@ -414,6 +472,18 @@ LaneChangeBTModule::LaneChangeBTModule( std::make_unique(parameters, LaneChangeModuleType::NORMAL, Direction::NONE)} { } + +void LaneChangeBTModule::updateRTCStatus(const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), start_distance, finish_distance, clock_->now()); +} + ExternalRequestLaneChangeLeftBTModule::ExternalRequestLaneChangeLeftBTModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index ec935edb91d9e..69260c97fcc02 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -24,6 +24,7 @@ namespace behavior_path_planner { using route_handler::Direction; +using utils::convertToSnakeCase; LaneChangeModuleManager::LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, std::shared_ptr parameters, const Direction direction, @@ -62,4 +63,33 @@ void LaneChangeModuleManager::updateModuleParams( }); } +AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( + rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, + std::shared_ptr parameters, + std::shared_ptr avoidance_parameters, + std::shared_ptr avoidance_by_lane_change_parameters) +: LaneChangeModuleManager( + node, name, config, std::move(parameters), Direction::NONE, + LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE), + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_by_lane_change_parameters_(std::move(avoidance_by_lane_change_parameters)) +{ + rtc_interface_ptr_map_.clear(); + const std::vector rtc_types = {"left", "right"}; + for (const auto & rtc_type : rtc_types) { + const auto snake_case_name = convertToSnakeCase(name); + const std::string rtc_interface_name = snake_case_name + "_" + rtc_type; + rtc_interface_ptr_map_.emplace( + rtc_type, std::make_shared(node, rtc_interface_name)); + } +} + +std::shared_ptr +AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +{ + return std::make_shared( + name_, *node_, parameters_, avoidance_parameters_, avoidance_by_lane_change_parameters_, + rtc_interface_ptr_map_); +} + } // namespace behavior_path_planner 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 3ec87716f8035..419a02470ff8b 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 @@ -59,18 +59,19 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) const auto current_lanes = getCurrentLanes(); if (current_lanes.empty()) { - return std::make_pair(false, false); + return {false, false}; } - const auto target_lanes = getLaneChangeLanes(current_lanes); + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); if (target_lanes.empty()) { - return std::make_pair(false, false); + return {false, false}; } // find candidate paths LaneChangePaths valid_paths{}; - const auto found_safe_path = getLaneChangePaths(current_lanes, target_lanes, &valid_paths); + const auto found_safe_path = + getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths); if (valid_paths.empty()) { return {false, false}; @@ -133,7 +134,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto & lane_change_end = status_.lane_change_path.shift_line.end; const double dist_to_lane_change_end = motion_utils::calcSignedArcLength( lane_change_path.points, current_pose.position, lane_change_end.position); - return dist_to_lane_change_end + parameters_->lane_change_finish_judge_buffer < 0.0; + return dist_to_lane_change_end + lane_change_parameters_->lane_change_finish_judge_buffer < 0.0; } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -145,7 +146,7 @@ bool NormalLaneChange::isCancelConditionSatisfied() { current_lane_change_state_ = LaneChangeStates::Normal; - if (!parameters_->enable_cancel_lane_change) { + if (!lane_change_parameters_->enable_cancel_lane_change) { return false; } @@ -167,7 +168,7 @@ bool NormalLaneChange::isCancelConditionSatisfied() return true; } - if (!parameters_->enable_abort_lane_change) { + if (!lane_change_parameters_->enable_abort_lane_change) { return false; } @@ -182,7 +183,7 @@ bool NormalLaneChange::isAbortConditionSatisfied(const Pose & pose) const auto & common_parameters = planner_data_->parameters; const auto found_abort_path = utils::lane_change::getAbortPaths( - planner_data_, status_.lane_change_path, pose, common_parameters, *parameters_); + planner_data_, status_.lane_change_path, pose, common_parameters, *lane_change_parameters_); if (!found_abort_path && !is_abort_path_approved_) { current_lane_change_state_ = LaneChangeStates::Stop; @@ -235,7 +236,7 @@ lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const } lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes) const + const lanelet::ConstLanelets & current_lanes, Direction direction) const { if (current_lanes.empty()) { return {}; @@ -256,7 +257,7 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( route_handler->getLaneletSequence(current_lane, getEgoPose(), 0.0, lane_change_prepare_length); const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( - *getRouteHandler(), current_lanes, type_, direction_); + *getRouteHandler(), current_lanes, type_, direction); const auto lane_change_lane_length = std::max(lane_change_lane_length_, getEgoVelocity() * 10.0); if (lane_change_lane) { @@ -297,7 +298,7 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - LaneChangePaths * candidate_paths) const + Direction direction, LaneChangePaths * candidate_paths) const { object_debug_.clear(); if (original_lanelets.empty() || target_lanelets.empty()) { @@ -314,7 +315,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto prepare_duration = common_parameter.lane_change_prepare_duration; const auto minimum_prepare_length = common_parameter.minimum_prepare_length; const auto minimum_lane_changing_velocity = common_parameter.minimum_lane_changing_velocity; - const auto lane_change_sampling_num = parameters_->lane_change_sampling_num; + const auto lane_change_sampling_num = lane_change_parameters_->lane_change_sampling_num; // get velocity const auto current_velocity = getEgoTwist().linear.x; @@ -425,7 +426,7 @@ bool NormalLaneChange::getLaneChangePaths( const double s_goal = lanelet::utils::getArcCoordinates(target_lanelets, route_handler.getGoalPose()).length; if ( - s_start + lane_changing_length + parameters_->lane_change_finish_judge_buffer + + s_start + lane_changing_length + lane_change_parameters_->lane_change_finish_judge_buffer + next_lane_change_buffer > s_goal) { RCLCPP_DEBUG( @@ -470,7 +471,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto candidate_path = utils::lane_change::constructCandidatePath( prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets, target_lanelets, sorted_lane_ids, acceleration, lc_length, lc_velocity, common_parameter, - *parameters_); + *lane_change_parameters_); if (!candidate_path) { RCLCPP_DEBUG( @@ -481,7 +482,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_valid = utils::lane_change::hasEnoughLength( *candidate_path, original_lanelets, target_lanelets, getEgoPose(), route_handler, - minimum_lane_changing_velocity, common_parameter, direction_); + minimum_lane_changing_velocity, common_parameter, direction); if (!is_valid) { RCLCPP_DEBUG( @@ -497,13 +498,14 @@ bool NormalLaneChange::getLaneChangePaths( route_handler, target_lanelets, getEgoPose(), check_length_); dynamic_object_indices = utils::lane_change::filterObjectIndices( {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, - getEgoPose(), common_parameter.forward_path_length, *parameters_, lateral_buffer); + getEgoPose(), common_parameter.forward_path_length, *lane_change_parameters_, + lateral_buffer); } candidate_paths->push_back(*candidate_path); const auto is_safe = utils::lane_change::isLaneChangePathSafe( *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(), - common_parameter, *parameters_, common_parameter.expected_front_deceleration, + common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration, common_parameter.expected_rear_deceleration, ego_pose_before_collision, object_debug_, acceleration); @@ -528,7 +530,7 @@ bool NormalLaneChange::isApprovedPathSafe(Pose & ego_pose_before_collision) cons const auto current_twist = getEgoTwist(); const auto & dynamic_objects = planner_data_->dynamic_object; const auto & common_parameters = planner_data_->parameters; - const auto & lane_change_parameters = *parameters_; + const auto & lane_change_parameters = *lane_change_parameters_; const auto & route_handler = planner_data_->route_handler; const auto & path = status_.lane_change_path; @@ -545,7 +547,7 @@ bool NormalLaneChange::isApprovedPathSafe(Pose & ego_pose_before_collision) cons return utils::lane_change::isLaneChangePathSafe( path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters, - *parameters_, common_parameters.expected_front_deceleration_for_abort, + *lane_change_parameters_, common_parameters.expected_front_deceleration_for_abort, common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, status_.lane_change_path.acceleration); } diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 67cdd72511585..aa4868c02792e 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -194,236 +194,6 @@ std::optional constructCandidatePath( return std::optional{candidate_path}; } -#ifdef USE_OLD_ARCHITECTURE -bool getLaneChangePaths( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, - const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data) -#else -bool getLaneChangePaths( - const PathWithLaneId & original_path, const RouteHandler & route_handler, - const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects, - const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, - const double check_length, const Direction direction, LaneChangePaths * candidate_paths, - std::unordered_map * debug_data) -#endif -{ - debug_data->clear(); - if (original_lanelets.empty() || target_lanelets.empty()) { - return false; - } - - Pose ego_pose_before_collision{}; - - // rename parameter - const auto backward_path_length = common_parameter.backward_path_length; - const auto forward_path_length = common_parameter.forward_path_length; - const auto prepare_duration = common_parameter.lane_change_prepare_duration; - const auto minimum_lane_changing_velocity = common_parameter.minimum_lane_changing_velocity; - const auto lane_change_sampling_num = parameter.lane_change_sampling_num; - - // get velocity - const auto current_velocity = twist.linear.x; - - // compute maximum_deceleration - const auto maximum_deceleration = std::invoke( - [&minimum_lane_changing_velocity, ¤t_velocity, &common_parameter, ¶meter]() { - const double min_a = (minimum_lane_changing_velocity - current_velocity) / - common_parameter.lane_change_prepare_duration; - return std::clamp( - min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits::epsilon()); - }); - - const auto acceleration_resolution = std::abs(maximum_deceleration) / lane_change_sampling_num; - - const auto target_length = - utils::getArcLengthToTargetLanelet(original_lanelets, target_lanelets.front(), pose); - -#ifdef USE_OLD_ARCHITECTURE - const auto shift_intervals = - route_handler.getLateralIntervalsToPreferredLane(target_lanelets.back()); -#else - const auto get_opposite_direction = - (direction == Direction::RIGHT) ? Direction::LEFT : Direction::RIGHT; - const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane( - target_lanelets.back(), get_opposite_direction); -#endif - const auto next_lane_change_buffer = - utils::calcMinimumLaneChangeLength(common_parameter, shift_intervals); - - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanelets.back()); - - const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(pose, original_lanelets); - - [[maybe_unused]] const auto arc_position_from_current = - lanelet::utils::getArcCoordinates(original_lanelets, pose); - const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, pose); - - const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); - - const auto sorted_lane_ids = getSortedLaneIds( - route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance); - const auto lateral_buffer = calcLateralBufferForFiltering(common_parameter.vehicle_width, 0.5); - - LaneChangeTargetObjectIndices dynamic_object_indices; - - candidate_paths->reserve(lane_change_sampling_num); - for (double sampled_acc = 0.0; sampled_acc >= maximum_deceleration; - sampled_acc -= acceleration_resolution) { - const auto prepare_velocity = - std::max(current_velocity + sampled_acc * prepare_duration, minimum_lane_changing_velocity); - - // compute actual acceleration - const double acceleration = (prepare_velocity - current_velocity) / prepare_duration; - - // get path on original lanes - const double prepare_length = - current_velocity * prepare_duration + 0.5 * acceleration * std::pow(prepare_duration, 2); - - if (prepare_length < target_length) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "prepare length is shorter than distance to target lane!!"); - break; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto prepare_segment = getPrepareSegment( - route_handler, original_lanelets, arc_position_from_current.length, backward_path_length, - prepare_length, prepare_velocity); -#else - const auto prepare_segment = getPrepareSegment( - original_path, original_lanelets, pose, backward_path_length, prepare_length, - std::max(prepare_velocity, minimum_lane_changing_velocity)); -#endif - - if (prepare_segment.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "prepare segment is empty!!"); - continue; - } - - // lane changing start pose is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - original_lanelets, target_lanelets.front(), lane_changing_start_pose); - // In new architecture, there is a possibility that the lane change start pose is behind of the - // target lanelet, even if the condition prepare_length > target_length is satisfied. In - // that case, the lane change shouldn't be executed. - if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "[only new arch] lane change start pose is behind target lanelet!!"); - break; - } - - // calculate shift length - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose); - - // we assume constant velocity during lane change - const auto lane_changing_velocity = prepare_velocity; - const auto lane_changing_length = - calcLaneChangingLength(lane_changing_velocity, shift_length, common_parameter); - - if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "lane changing path too long"); - continue; - } - - const auto target_segment = getTargetSegment( - route_handler, target_lanelets, forward_path_length, lane_changing_start_pose, - target_lane_length, lane_changing_length, lane_changing_velocity, next_lane_change_buffer); - - if (target_segment.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "target segment is empty!! something wrong..."); - continue; - } - - const auto resample_interval = - calcLaneChangeResampleInterval(lane_changing_length, lane_changing_velocity); - - const auto lc_length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - const auto target_lane_reference_path = getReferencePathFromTargetLane( - route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, - lc_length.lane_changing, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); - - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "target_lane_reference_path is empty!!"); - continue; - } - - const auto shift_line = getLaneChangingShiftLine( - prepare_segment, target_segment, target_lane_reference_path, shift_length); - - const auto lc_velocity = LaneChangePhaseInfo{prepare_velocity, lane_changing_velocity}; - - const auto candidate_path = constructCandidatePath( - prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets, - target_lanelets, sorted_lane_ids, acceleration, lc_length, lc_velocity, common_parameter, - parameter); - - if (!candidate_path) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "no candidate path!!"); - continue; - } - -#ifdef USE_OLD_ARCHITECTURE - const auto is_valid = hasEnoughLength( - *candidate_path, original_lanelets, target_lanelets, pose, route_handler, - minimum_lane_changing_velocity, common_parameter); -#else - const auto is_valid = hasEnoughLength( - *candidate_path, original_lanelets, target_lanelets, pose, route_handler, - minimum_lane_changing_velocity, common_parameter, direction); -#endif - - if (!is_valid) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util").get_child("lane_change"), - "invalid candidate path!!"); - continue; - } - - if (candidate_paths->empty()) { - // only compute dynamic object indices once - const auto backward_target_lanes_for_object_filtering = - utils::lane_change::getBackwardLanelets(route_handler, target_lanelets, pose, check_length); - dynamic_object_indices = filterObjectIndices( - {*candidate_path}, *dynamic_objects, backward_target_lanes_for_object_filtering, pose, - common_parameter.forward_path_length, parameter, lateral_buffer); - } - candidate_paths->push_back(*candidate_path); - - const auto is_safe = isLaneChangePathSafe( - *candidate_path, dynamic_objects, dynamic_object_indices, pose, twist, common_parameter, - parameter, common_parameter.expected_front_deceleration, - common_parameter.expected_rear_deceleration, ego_pose_before_collision, *debug_data, - acceleration); - - if (is_safe) { - return true; - } - } - - return false; -} - bool hasEnoughLength( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, From 8b79207c1d6450fe1d3bc0d3d580efd1c32dc56c Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 8 May 2023 11:38:04 +0900 Subject: [PATCH 05/21] feat(behavior_path_planner): run fixed_goal_planner with other modules simutaneously (#3574) Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 2 +- .../scene_module/scene_module_interface.hpp | 23 +++++++++++++++ .../scene_module_manager_interface.hpp | 14 +++++++-- .../default_fixed_goal_planner.hpp | 2 ++ .../goal_planner/fixed_goal_planner_base.hpp | 10 +++++++ .../goal_planner/goal_planner_module.cpp | 24 ++++++++++++--- .../default_fixed_goal_planner.cpp | 29 +++++++++++++------ 7 files changed, 88 insertions(+), 16 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index c458fd16a7073..6cdde400d63c4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -179,7 +179,7 @@ class GoalPlannerModule : public SceneModuleInterface bool isStopped( std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(); - bool hasFinishedPullOver(); + bool hasFinishedGoalPlanner(); void updateOccupancyGrid(); void resetStatus(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index e08116928c9e4..82f60a049883b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -378,6 +378,26 @@ class SceneModuleInterface rclcpp::Logger getLogger() const { return logger_; } + void setIsSimultaneousExecutableAsApprovedModule(const bool enable) + { + is_simultaneously_executable_as_approved_module_ = enable; + } + + bool isSimultaneousExecutableAsApprovedModule() const + { + return is_simultaneously_executable_as_approved_module_; + } + + void setIsSimultaneousExecutableAsCandidateModule(const bool enable) + { + is_simultaneously_executable_as_candidate_module_ = enable; + } + + bool isSimultaneousExecutableAsCandidateModule() const + { + return is_simultaneously_executable_as_candidate_module_; + } + private: std::string name_; @@ -463,6 +483,9 @@ class SceneModuleInterface return expanded_lanes; } + bool is_simultaneously_executable_as_approved_module_{false}; + bool is_simultaneously_executable_as_candidate_module_{false}; + rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index 44606dc728197..c99ec9743756e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -94,6 +94,10 @@ class SceneModuleManagerInterface void registerNewModule( const SceneModulePtr & module_ptr, const BehaviorModuleOutput & previous_module_output) { + module_ptr->setIsSimultaneousExecutableAsApprovedModule( + enable_simultaneous_execution_as_approved_module_); + module_ptr->setIsSimultaneousExecutableAsCandidateModule( + enable_simultaneous_execution_as_candidate_module_); module_ptr->setData(planner_data_); module_ptr->setPreviousModuleOutput(previous_module_output); module_ptr->onEntry(); @@ -197,12 +201,18 @@ class SceneModuleManagerInterface bool isSimultaneousExecutableAsApprovedModule() const { - return enable_simultaneous_execution_as_approved_module_; + return std::all_of( + registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) { + return module->isSimultaneousExecutableAsApprovedModule(); + }); } bool isSimultaneousExecutableAsCandidateModule() const { - return enable_simultaneous_execution_as_candidate_module_; + return std::all_of( + registered_modules_.begin(), registered_modules_.end(), [](const SceneModulePtr & module) { + return module->isSimultaneousExecutableAsCandidateModule(); + }); } void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp index bca97847c7960..568ecdc98c705 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp @@ -32,8 +32,10 @@ class DefaultFixedGoalPlanner : public FixedGoalPlannerBase BehaviorModuleOutput plan(const std::shared_ptr & planner_data) const override; protected: +#ifdef USE_OLD_ARCHITECTURE boost::optional getReferencePath( const std::shared_ptr & planner_data) const; +#endif PathWithLaneId modifyPathForSmoothGoalConnection( const PathWithLaneId & path, const std::shared_ptr & planner_data) const; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp index 0840b5bb78008..3bc0e960fe468 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp @@ -39,6 +39,16 @@ class FixedGoalPlannerBase virtual BehaviorModuleOutput plan( const std::shared_ptr & planner_data) const = 0; + + void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + { + previous_module_output_ = previous_module_output; + } + + BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } + +protected: + BehaviorModuleOutput previous_module_output_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index df2bef8dafb4c..829134411e39b 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -421,6 +421,11 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const ModuleStatus GoalPlannerModule::updateState() { + // finish module only when the goal is fixed + if (!allow_goal_modification_ && hasFinishedGoalPlanner()) { + current_state_ = ModuleStatus::SUCCESS; + } + // pull_out module will be run when setting new goal, so not need finishing pull_over module. // Finishing it causes wrong lane_following path generation. return current_state_; @@ -489,6 +494,11 @@ BehaviorModuleOutput GoalPlannerModule::plan() if (allow_goal_modification_) { return planWithGoalModification(); } else { + // for fixed goals, only minor path refinements are made, + // so other modules are always allowed to run. + setIsSimultaneousExecutableAsApprovedModule(true); + setIsSimultaneousExecutableAsCandidateModule(true); + fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } } @@ -756,6 +766,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() if (allow_goal_modification_) { return planWaitingApprovalWithGoalModification(); } else { + // for fixed goals, only minor path refinements are made, + // so other modules are always allowed to run. + setIsSimultaneousExecutableAsApprovedModule(true); + setIsSimultaneousExecutableAsCandidateModule(true); + fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); return fixed_goal_planner_->plan(planner_data_); } } @@ -1001,12 +1016,13 @@ bool GoalPlannerModule::hasFinishedCurrentPath() bool GoalPlannerModule::isOnGoal() const { - const auto current_pose = planner_data_->self_odometry->pose.pose; - return calcDistance2d(current_pose, modified_goal_pose_->goal_pose) < - parameters_->th_arrived_distance; + const Pose current_pose = planner_data_->self_odometry->pose.pose; + const Pose goal_pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose + : planner_data_->route_handler->getGoalPose(); + return calcDistance2d(current_pose, goal_pose) < parameters_->th_arrived_distance; } -bool GoalPlannerModule::hasFinishedPullOver() +bool GoalPlannerModule::hasFinishedGoalPlanner() { return isOnGoal() && isStopped(); } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp index a33217d2551b3..085f72b59d31e 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -29,18 +29,28 @@ namespace behavior_path_planner BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const { - auto output = getReferencePath(planner_data); - if (!output) { - return {}; - } + BehaviorModuleOutput output = +#ifdef USE_OLD_ARCHITECTURE + // generate reference path in this planner + std::invoke([this, &planner_data]() { + auto path = getReferencePath(planner_data); + if (!path) { + return BehaviorModuleOutput{}; + } + return *path; + }); +#else + // use planner previous module reference path + getPreviousModuleOutput(); +#endif const PathWithLaneId smoothed_path = - modifyPathForSmoothGoalConnection(*(output->path), planner_data); - - output->path = std::make_shared(smoothed_path); - output->reference_path = std::make_shared(smoothed_path); - return *output; + modifyPathForSmoothGoalConnection(*(output.path), planner_data); + output.path = std::make_shared(smoothed_path); + output.reference_path = std::make_shared(smoothed_path); + return output; } +#ifdef USE_OLD_ARCHITECTURE boost::optional DefaultFixedGoalPlanner::getReferencePath( const std::shared_ptr & planner_data) const { @@ -61,6 +71,7 @@ boost::optional DefaultFixedGoalPlanner::getReferencePath( return {}; // something wrong } +#endif PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( const PathWithLaneId & path, const std::shared_ptr & planner_data) const From 3744deea7549ebe002320304de59574f72e7ae6e Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 8 May 2023 12:11:57 +0900 Subject: [PATCH 06/21] test(obstacle_velocity_limiter): add off-track test (#3588) Signed-off-by: Takamasa Horibe --- ...stacle_velocity_limiter_node_interface.cpp | 54 +++++++++++++++---- 1 file changed, 44 insertions(+), 10 deletions(-) diff --git a/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp b/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp index e91e229ede9fd..c2ffaffc75287 100644 --- a/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp +++ b/planning/obstacle_velocity_limiter/test/test_obstacle_velocity_limiter_node_interface.cpp @@ -21,12 +21,20 @@ #include -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) -{ - rclcpp::init(0, nullptr); +using obstacle_velocity_limiter::ObstacleVelocityLimiterNode; +using planning_test_utils::PlanningInterfaceTestManager; - auto test_manager = std::make_shared(); +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + test_manager->setTrajectorySubscriber("obstacle_velocity_limiter/output/trajectory"); + test_manager->setTrajectoryInputTopicName("obstacle_velocity_limiter/input/trajectory"); + test_manager->setOdometryTopicName("obstacle_velocity_limiter/input/odometry"); + return test_manager; +} +std::shared_ptr generateNode() +{ auto node_options = rclcpp::NodeOptions{}; const auto planning_test_utils_dir = @@ -40,9 +48,13 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", obstacle_velocity_limiter_dir + "/config/default_obstacle_velocity_limiter.param.yaml"}); - auto test_target_node = - std::make_shared(node_options); + return std::make_shared(node_options); +} +void publishMandatoryTopics( + std::shared_ptr test_manager, + rclcpp::Node::SharedPtr test_target_node) +{ // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "obstacle_velocity_limiter/input/odometry"); test_manager->publishPointCloud( @@ -52,12 +64,15 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) test_manager->publishPredictedObjects( test_target_node, "obstacle_velocity_limiter/input/dynamic_obstacles"); test_manager->publishMap(test_target_node, "obstacle_velocity_limiter/input/map"); +} - // set subscriber with topic name: obstacle_velocity_limiter → test_node_ - test_manager->setTrajectorySubscriber("obstacle_velocity_limiter/output/trajectory"); +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); - // set obstacle_velocity_limiter's input topic name(this topic is changed to test node): - test_manager->setTrajectoryInputTopicName("obstacle_velocity_limiter/input/trajectory"); + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); @@ -66,4 +81,23 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) // test for trajectory with empty/one point/overlapping point ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); } From 364b457dddeb12c96a2e8651ff2c200c25286769 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 8 May 2023 13:11:09 +0900 Subject: [PATCH 07/21] test(obstacle_stop_planner): add off-track test (#3589) Signed-off-by: Takamasa Horibe --- ...t_obstacle_stop_planner_node_interface.cpp | 55 ++++++++++++++++--- 1 file changed, 46 insertions(+), 9 deletions(-) diff --git a/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp b/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp index dc646e35d09a6..e09f4b4ffe557 100644 --- a/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp +++ b/planning/obstacle_stop_planner/test/test_obstacle_stop_planner_node_interface.cpp @@ -21,12 +21,20 @@ #include -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) -{ - rclcpp::init(0, nullptr); +using motion_planning::ObstacleStopPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; - auto test_manager = std::make_shared(); +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + test_manager->setTrajectorySubscriber("obstacle_stop_planner/output/trajectory"); + test_manager->setTrajectoryInputTopicName("obstacle_stop_planner/input/trajectory"); + test_manager->setOdometryTopicName("obstacle_stop_planner/input/odometry"); + return test_manager; +} +std::shared_ptr generateNode() +{ auto node_options = rclcpp::NodeOptions{}; const auto planning_test_utils_dir = @@ -44,8 +52,13 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) obstacle_stop_planner_dir + "/config/adaptive_cruise_control.param.yaml", "--params-file", obstacle_stop_planner_dir + "/config/obstacle_stop_planner.param.yaml"}); - auto test_target_node = std::make_shared(node_options); + return std::make_shared(node_options); +} +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "obstacle_stop_planner/input/odometry"); test_manager->publishPointCloud(test_target_node, "obstacle_stop_planner/input/pointcloud"); @@ -53,12 +66,16 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) test_manager->publishPredictedObjects(test_target_node, "obstacle_stop_planner/input/objects"); test_manager->publishExpandStopRange( test_target_node, "obstacle_stop_planner/input/expand_stop_range"); +} - // set subscriber with topic name: obstacle stop planner → test_node_ - test_manager->setTrajectorySubscriber("obstacle_stop_planner/output/trajectory"); +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); - // set obstacle stop planner's input topic name(this topic is changed to test node): - test_manager->setTrajectoryInputTopicName("obstacle_stop_planner/input/trajectory"); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); @@ -67,4 +84,24 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) // test for trajectory with empty/one point/overlapping point test_manager->testWithAbnormalTrajectory(test_target_node); + + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); } From 022adddd549dcd1dd4f8aca033dfbd3a9c9d3cc4 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 8 May 2023 13:17:57 +0900 Subject: [PATCH 08/21] test(planning_validator): add off-track test (#3594) Signed-off-by: Takamasa Horibe --- ...test_planning_validator_node_interface.cpp | 50 ++++++++++++++++--- 1 file changed, 43 insertions(+), 7 deletions(-) diff --git a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp index 8a3e8d2abdb97..95619e749a9f9 100644 --- a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -21,10 +21,26 @@ #include -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +using planning_test_utils::PlanningInterfaceTestManager; +using planning_validator::PlanningValidator; + +std::shared_ptr generateTestManager() { auto test_manager = std::make_shared(); + // set subscriber with topic name: planning_validator → test_node_ + test_manager->setTrajectorySubscriber("planning_validator/output/trajectory"); + + // set planning_validator's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("planning_validator/input/trajectory"); + + test_manager->setOdometryTopicName("planning_validator/input/kinematics"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ auto node_options = rclcpp::NodeOptions{}; const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); @@ -34,16 +50,23 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", planning_validator_dir + "/config/planning_validator.param.yaml"}); - auto test_target_node = std::make_shared(node_options); + return std::make_shared(node_options); +} +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "planning_validator/input/kinematics"); +} - // set subscriber with topic name: planning_validator → test_node_ - test_manager->setTrajectorySubscriber("planning_validator/output/trajectory"); +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); - // set planning_validator's input topic name(this topic is changed to test node) - test_manager->setTrajectoryInputTopicName("planning_validator/input/trajectory"); + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); @@ -51,6 +74,19 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) // test for trajectory with empty/one point/overlapping point ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); +} - rclcpp::shutdown(); +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); } From 6682ce0c624c8042f3dc65ad19acb0d48cd81ac8 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 8 May 2023 13:36:04 +0900 Subject: [PATCH 09/21] refactor(lane_change): remove avoidance_by_lc folder (#3632) Signed-off-by: Muhammad Zulfaqar --- .../scene_module/avoidance_by_lc/manager.hpp | 55 -- .../scene_module/avoidance_by_lc/module.hpp | 216 ------ .../scene_module/avoidance_by_lc/manager.cpp | 47 -- .../scene_module/avoidance_by_lc/module.cpp | 617 ------------------ 4 files changed, 935 deletions(-) delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp delete mode 100644 planning/behavior_path_planner/src/scene_module/avoidance_by_lc/manager.cpp delete mode 100644 planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp deleted file mode 100644 index 4727275a6d4ec..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MANAGER_HPP_ - -#include "behavior_path_planner/scene_module/avoidance_by_lc/module.hpp" -#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" -#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp" - -#include - -#include -#include -#include -#include - -namespace behavior_path_planner -{ - -class AvoidanceByLCModuleManager : public SceneModuleManagerInterface -{ -public: - AvoidanceByLCModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters); - - std::shared_ptr createNewSceneModuleInstance() override - { - return std::make_shared( - name_, *node_, parameters_, rtc_interface_ptr_map_); - } - - void updateModuleParams(const std::vector & parameters) override; - -private: - std::shared_ptr parameters_; - - std::vector> registered_modules_; -}; - -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp deleted file mode 100644 index e6badb01e716d..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance_by_lc/module.hpp +++ /dev/null @@ -1,216 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MODULE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MODULE_HPP_ - -#include "behavior_path_planner/marker_util/lane_change/debug.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/avoidance_by_lc/module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" - -#include - -#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; -using route_handler::Direction; -using tier4_planning_msgs::msg::LaneChangeDebugMsg; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; - -class AvoidanceByLCModule : public SceneModuleInterface -{ -public: - AvoidanceByLCModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map); - - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - ModuleStatus updateState() override; - BehaviorModuleOutput plan() override; - BehaviorModuleOutput planWaitingApproval() override; - CandidateOutput planCandidate() const override; - void processOnEntry() override; - void processOnExit() override; - void updateData() override; - - std::shared_ptr get_debug_msg_array() const; - void acceptVisitor(const std::shared_ptr & visitor) const override; - -#ifndef USE_OLD_ARCHITECTURE - void updateModuleParams(const std::shared_ptr & parameters) - { - parameters_ = parameters; - } -#endif - -private: - std::shared_ptr parameters_; - LaneChangeStatus status_; - PathShifter path_shifter_; - mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; - LaneChangeStates current_lane_change_state_; - std::shared_ptr abort_path_; - PathWithLaneId prev_approved_path_; - - double lane_change_lane_length_{200.0}; - double check_distance_{100.0}; - bool is_abort_path_approved_{false}; - bool is_abort_approval_requested_{false}; - bool is_abort_condition_satisfied_{false}; - bool is_activated_ = false; - - void resetParameters(); - - void waitApprovalLeft(const double start_distance, const double finish_distance) - { - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - uuid_map_.at("left"), isExecutionReady(), start_distance, finish_distance, clock_->now()); - is_waiting_approval_ = true; - } - - void waitApprovalRight(const double start_distance, const double finish_distance) - { - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - uuid_map_.at("right"), isExecutionReady(), start_distance, finish_distance, clock_->now()); - is_waiting_approval_ = true; - } - - void updateRTCStatus(const CandidateOutput & candidate) - { - if (candidate.lateral_shift > 0.0) { - rtc_interface_ptr_map_.at("left")->updateCooperateStatus( - uuid_map_.at("left"), isExecutionReady(), candidate.start_distance_to_path_change, - candidate.finish_distance_to_path_change, clock_->now()); - return; - } - if (candidate.lateral_shift < 0.0) { - rtc_interface_ptr_map_.at("right")->updateCooperateStatus( - uuid_map_.at("right"), isExecutionReady(), candidate.start_distance_to_path_change, - candidate.finish_distance_to_path_change, clock_->now()); - return; - } - - RCLCPP_WARN_STREAM( - getLogger(), - "Direction is UNKNOWN, start_distance = " << candidate.start_distance_to_path_change); - } - - lanelet::ConstLanelets getCurrentLanes(const PathWithLaneId & path) const - { - const auto & route_handler = planner_data_->route_handler; - const auto & common_parameters = planner_data_->parameters; - - lanelet::ConstLanelets reference_lanes{}; - for (const auto & p : path.points) { - reference_lanes.push_back( - planner_data_->route_handler->getLaneletsFromId(p.lane_ids.front())); - } - - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(reference_lanes, getEgoPose(), ¤t_lane); - - return route_handler->getLaneletSequence( - current_lane, getEgoPose(), common_parameters.backward_path_length, - common_parameters.forward_path_length); - } - - void removePreviousRTCStatusLeft() - { - if (rtc_interface_ptr_map_.at("left")->isRegistered(uuid_map_.at("left"))) { - rtc_interface_ptr_map_.at("left")->removeCooperateStatus(uuid_map_.at("left")); - } - } - - void removePreviousRTCStatusRight() - { - if (rtc_interface_ptr_map_.at("right")->isRegistered(uuid_map_.at("right"))) { - rtc_interface_ptr_map_.at("right")->removeCooperateStatus(uuid_map_.at("right")); - } - } - - AvoidancePlanningData calcAvoidancePlanningData(DebugData & debug) const; - AvoidancePlanningData avoidance_data_; - mutable DebugData debug_data_; - - ObjectDataArray registered_objects_; - mutable ObjectDataArray stopped_objects_; - - ObjectData createObjectData( - const AvoidancePlanningData & data, const PredictedObject & object) const; - - void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const; - - lanelet::ConstLanelets get_original_lanes() const; - PathWithLaneId getReferencePath() const; - lanelet::ConstLanelets getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; - std::pair getSafePath( - const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, - LaneChangePath & safe_path) const; - - void updateLaneChangeStatus(); - void generateExtendedDrivableArea(PathWithLaneId & path); - void updateOutputTurnSignal(BehaviorModuleOutput & output); - void updateSteeringFactorPtr(const BehaviorModuleOutput & output); - bool isApprovedPathSafe(Pose & ego_pose_before_collision) const; - void calcTurnSignalInfo(); - - void updateSteeringFactorPtr( - const CandidateOutput & output, const LaneChangePath & selected_path) const; - bool isSafe() const; - bool isValidPath() const; - bool isValidPath(const PathWithLaneId & path) const; - bool isNearEndOfLane() const; - bool isCurrentVelocityLow() const; - bool isAbortConditionSatisfied(); - bool hasFinishedLaneChange() const; - bool isAvoidancePlanRunning() const; - bool isAbortState() const; - - // getter - std_msgs::msg::Header getRouteHeader() const; - void resetPathIfAbort(); - - // debug - void setObjectDebugVisualization() const; - mutable std::unordered_map object_debug_; - mutable std::vector debug_valid_path_; -}; -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE_BY_LC__MODULE_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/manager.cpp deleted file mode 100644 index 3e756ddd99149..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/manager.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "behavior_path_planner/scene_module/avoidance_by_lc/manager.hpp" - -#include - -#include -#include -#include - -namespace behavior_path_planner -{ - -AvoidanceByLCModuleManager::AvoidanceByLCModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::shared_ptr & parameters) -: SceneModuleManagerInterface(node, name, config, {"left", "right"}), parameters_{parameters} -{ -} - -void AvoidanceByLCModuleManager::updateModuleParams( - [[maybe_unused]] const std::vector & parameters) -{ - using tier4_autoware_utils::updateParam; - - [[maybe_unused]] auto p = parameters_; - - [[maybe_unused]] const std::string ns = name_ + "."; - - std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { - m->updateModuleParams(p); - }); -} - -} // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp deleted file mode 100644 index c76034d03cecb..0000000000000 --- a/planning/behavior_path_planner/src/scene_module/avoidance_by_lc/module.cpp +++ /dev/null @@ -1,617 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "behavior_path_planner/scene_module/avoidance_by_lc/module.hpp" - -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/avoidance/utils.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" - -#include -#include -#include - -#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace behavior_path_planner -{ - -using autoware_auto_perception_msgs::msg::ObjectClassification; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::toHexString; - -AvoidanceByLCModule::AvoidanceByLCModule( - const std::string & name, rclcpp::Node & node, - const std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} -{ -} -BehaviorModuleOutput AvoidanceByLCModule::planWaitingApproval() -{ - prev_approved_path_ = *getPreviousModuleOutput().path; - BehaviorModuleOutput out; - out.path = std::make_shared(prev_approved_path_); - out.reference_path = getPreviousModuleOutput().reference_path; - out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - - // for new architecture - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); - const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(*out.path, drivable_lanes); - out.drivable_area_info.drivable_lanes = utils::combineDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes, target_drivable_lanes); - - if (!avoidance_data_.target_objects.empty()) { - const auto to_front_object_distance = avoidance_data_.target_objects.front().longitudinal; - const double shift_length = status_.lane_change_path.shift_line.end_shift_length - - status_.lane_change_path.shift_line.start_shift_length; - const double lane_change_buffer = - utils::calcMinimumLaneChangeLength(planner_data_->parameters, {shift_length}); - - utils::avoidance::insertDecelPoint( - getEgoPosition(), to_front_object_distance - lane_change_buffer, 0.0, *out.path, stop_pose_); - } - - updateLaneChangeStatus(); - - const auto candidate = planCandidate(); - path_candidate_ = std::make_shared(candidate.path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; - updateRTCStatus(candidate); - waitApproval(); - is_abort_path_approved_ = false; - - return out; -} - -void AvoidanceByLCModule::updateLaneChangeStatus() -{ -#ifdef USE_OLD_ARCHITECTURE - status_.current_lanes = utils::getCurrentLanes(planner_data_); -#else - status_.current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - status_.lane_change_lanes = getLaneChangeLanes(status_.current_lanes, lane_change_lane_length_); - - // Find lane change path - LaneChangePath selected_path; - const auto [found_valid_path, found_safe_path] = - getSafePath(status_.lane_change_lanes, check_distance_, selected_path); - - // Update status - status_.is_safe = found_safe_path; - status_.lane_change_path = selected_path; - status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); - status_.lane_change_lane_ids = utils::getIds(status_.lane_change_lanes); - - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.lane_change_lanes, getEgoPose()); - status_.start_distance = arclength_start.length; - status_.lane_change_path.path.header = getRouteHeader(); -} - -PathWithLaneId AvoidanceByLCModule::getReferencePath() const -{ - PathWithLaneId reference_path; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto & common_parameters = planner_data_->parameters; - - // Set header - reference_path.header = getRouteHeader(); - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - - if (current_lanes.empty()) { - return reference_path; - } - - if (reference_path.points.empty()) { - reference_path = utils::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters); - } - - reference_path = utils::getCenterLinePath( - *route_handler, current_lanes, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length, common_parameters, 0.0); - - const auto shift_intervals = - route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double lane_change_buffer = - utils::calcMinimumLaneChangeLength(common_parameters, shift_intervals); - - reference_path = utils::setDecelerationVelocity( - *route_handler, reference_path, current_lanes, common_parameters.lane_change_prepare_duration, - lane_change_buffer); - - const auto drivable_lanes = utils::generateDrivableLanes(current_lanes); - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(reference_path, drivable_lanes); - utils::generateDrivableArea( - reference_path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_); - - return reference_path; -} - -lanelet::ConstLanelets AvoidanceByLCModule::getLaneChangeLanes( - const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const -{ - lanelet::ConstLanelets lane_change_lanes; - const auto & route_handler = planner_data_->route_handler; - const auto minimum_prepare_length = planner_data_->parameters.minimum_prepare_length; - const auto prepare_duration = planner_data_->parameters.lane_change_prepare_duration; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - - if (current_lanes.empty()) { - return lane_change_lanes; - } - - const auto object_num = avoidance_data_.target_objects.size(); - if (object_num < parameters_->execute_object_num) { - return lane_change_lanes; - } - - const auto o_front = avoidance_data_.target_objects.front(); - - // Get lane change lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); - const double lane_change_prepare_length = - std::max(current_twist.linear.x * prepare_duration, minimum_prepare_length); - lanelet::ConstLanelets current_check_lanes = - route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length); - lanelet::ConstLanelet lane_change_lane; - - if (utils::avoidance::isOnRight(o_front)) { - for (const auto & lanelet : current_check_lanes) { - const auto & left_lane = route_handler->getRoutingGraphPtr()->left(lanelet); - if (left_lane) { - lane_change_lanes = route_handler->getLaneletSequence( - left_lane.get(), current_pose, lane_change_lane_length, lane_change_lane_length); - break; - } - } - } else { - for (const auto & lanelet : current_check_lanes) { - const auto & right_lane = route_handler->getRoutingGraphPtr()->right(lanelet); - if (right_lane) { - lane_change_lanes = route_handler->getLaneletSequence( - right_lane.get(), current_pose, lane_change_lane_length, lane_change_lane_length); - break; - } - } - } - - return lane_change_lanes; -} - -std::pair AvoidanceByLCModule::getSafePath( - const lanelet::ConstLanelets & lane_change_lanes, const double check_distance, - LaneChangePath & safe_path) const -{ - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & common_parameters = planner_data_->parameters; - -#ifdef USE_OLD_ARCHITECTURE - const auto current_lanes = utils::getCurrentLanes(planner_data_); -#else - const auto current_lanes = - utils::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); -#endif - - if (lane_change_lanes.empty()) { - return std::make_pair(false, false); - } - - if (avoidance_data_.target_objects.empty()) { - return std::make_pair(false, false); - } - - // find candidate paths - LaneChangePaths valid_paths; -#ifdef USE_OLD_ARCHITECTURE - const auto found_safe_path = utils::lane_change::getLaneChangePaths( - *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, - planner_data_->dynamic_object, common_parameters, *parameters_->lane_change, check_distance, - &valid_paths, &object_debug_); -#else - const auto o_front = avoidance_data_.target_objects.front(); - const auto direction = utils::avoidance::isOnRight(o_front) ? Direction::LEFT : Direction::RIGHT; - const auto found_safe_path = utils::lane_change::getLaneChangePaths( - *getPreviousModuleOutput().path, *route_handler, current_lanes, lane_change_lanes, current_pose, - current_twist, planner_data_->dynamic_object, common_parameters, *parameters_->lane_change, - check_distance, direction, &valid_paths, &object_debug_); -#endif - debug_valid_path_ = valid_paths; - - if (parameters_->lane_change->publish_debug_marker) { - setObjectDebugVisualization(); - } else { - debug_marker_.markers.clear(); - } - - if (valid_paths.empty()) { - return {false, false}; - } - - if (found_safe_path) { - safe_path = valid_paths.back(); - } else { - // force candidate - safe_path = valid_paths.front(); - } - - return {true, found_safe_path}; -} - -bool AvoidanceByLCModule::isSafe() const -{ - return status_.is_safe; -} - -bool AvoidanceByLCModule::isValidPath() const -{ - return status_.is_valid_path; -} - -bool AvoidanceByLCModule::isValidPath(const PathWithLaneId & path) const -{ - const auto & route_handler = planner_data_->route_handler; - - // check lane departure - const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, utils::extendLanes(route_handler, status_.current_lanes), - utils::extendLanes(route_handler, status_.lane_change_lanes)); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); - const auto lanelets = utils::transformToLanelets(expanded_lanes); - - // check path points are in any lanelets - for (const auto & point : path.points) { - bool is_in_lanelet = false; - for (const auto & lanelet : lanelets) { - if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) { - is_in_lanelet = true; - break; - } - } - if (!is_in_lanelet) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); - return false; - } - } - - // check relative angle - if (!utils::checkPathRelativeAngle(path, M_PI)) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path relative angle is invalid"); - return false; - } - - return true; -} - -bool AvoidanceByLCModule::isNearEndOfLane() const -{ - const auto & current_pose = getEgoPose(); - const auto shift_intervals = - planner_data_->route_handler->getLateralIntervalsToPreferredLane(status_.current_lanes.back()); - const double threshold = - utils::calcMinimumLaneChangeLength(planner_data_->parameters, shift_intervals); - - return std::max(0.0, utils::getDistanceToEndOfLane(current_pose, status_.current_lanes)) < - threshold; -} - -bool AvoidanceByLCModule::isCurrentVelocityLow() const -{ - constexpr double threshold_ms = 10.0 * 1000 / 3600; - return utils::l2Norm(getEgoTwist().linear) < threshold_ms; -} - -bool AvoidanceByLCModule::isAbortConditionSatisfied() -{ - is_abort_condition_satisfied_ = false; - current_lane_change_state_ = LaneChangeStates::Normal; - - if (!parameters_->lane_change->enable_cancel_lane_change) { - return false; - } - - if (!is_activated_) { - return false; - } - - Pose ego_pose_before_collision; - const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision); - - if (!is_path_safe) { - const auto & common_parameters = planner_data_->parameters; - const bool is_within_original_lane = utils::lane_change::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), common_parameters); - - if (is_within_original_lane) { - current_lane_change_state_ = LaneChangeStates::Cancel; - return true; - } - - // check abort enable flag - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), *clock_, 1000, - "DANGER!!! Path is not safe anymore, but it is too late to CANCEL! Please be cautious"); - - if (!parameters_->lane_change->enable_abort_lane_change) { - current_lane_change_state_ = LaneChangeStates::Stop; - return false; - } - - const auto found_abort_path = utils::lane_change::getAbortPaths( - planner_data_, status_.lane_change_path, ego_pose_before_collision, common_parameters, - *parameters_->lane_change); - - if (!found_abort_path && !is_abort_path_approved_) { - current_lane_change_state_ = LaneChangeStates::Stop; - return true; - } - - current_lane_change_state_ = LaneChangeStates::Abort; - - if (!is_abort_path_approved_) { - abort_path_ = std::make_shared(*found_abort_path); - } - - return true; - } - - return false; -} - -bool AvoidanceByLCModule::isAbortState() const -{ - if (!parameters_->lane_change->enable_abort_lane_change) { - return false; - } - - if (current_lane_change_state_ != LaneChangeStates::Abort) { - return false; - } - - if (!abort_path_) { - return false; - } - - RCLCPP_WARN_STREAM_THROTTLE( - getLogger(), *clock_, 1000, - "DANGER!!! Lane change transition to ABORT state, return path will be computed!"); - return true; -} - -bool AvoidanceByLCModule::isAvoidancePlanRunning() const -{ - constexpr double AVOIDING_SHIFT_THR = 0.1; - - const auto & current_pose = getEgoPose(); - const auto arclength_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); - - return std::abs(arclength_current.distance) > AVOIDING_SHIFT_THR; -} - -bool AvoidanceByLCModule::hasFinishedLaneChange() const -{ - const auto & current_pose = getEgoPose(); - const auto arclength_current = - lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose); - const double travel_distance = arclength_current.length - status_.start_distance; - const double finish_distance = status_.lane_change_path.length.sum() + - parameters_->lane_change->lane_change_finish_judge_buffer; - return travel_distance > finish_distance; -} - -void AvoidanceByLCModule::setObjectDebugVisualization() const -{ - using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; - - debug_marker_.markers.clear(); - const auto add = [this](const MarkerArray & added) { - tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); - }; - - add(showObjectInfo(object_debug_, "object_debug_info")); - add(showLerpedPose(object_debug_, "lerp_pose_before_true")); - add(showPolygonPose(object_debug_, "expected_pose")); - add(showPolygon(object_debug_, "lerped_polygon")); - add(showAllValidLaneChangePath(debug_valid_path_, "lane_change_valid_paths")); -} - -std::shared_ptr AvoidanceByLCModule::get_debug_msg_array() const -{ - LaneChangeDebugMsgArray debug_msg_array; - debug_msg_array.lane_change_info.reserve(object_debug_.size()); - for (const auto & [uuid, debug_data] : object_debug_) { - LaneChangeDebugMsg debug_msg; - debug_msg.object_id = uuid; - debug_msg.allow_lane_change = debug_data.allow_lane_change; - debug_msg.is_front = debug_data.is_front; - debug_msg.relative_distance = debug_data.relative_to_ego; - debug_msg.failed_reason = debug_data.failed_reason; - debug_msg.velocity = utils::l2Norm(debug_data.object_twist.linear); - debug_msg_array.lane_change_info.push_back(debug_msg); - } - lane_change_debug_msg_array_ = debug_msg_array; - - lane_change_debug_msg_array_.header.stamp = clock_->now(); - return std::make_shared(lane_change_debug_msg_array_); -} - -void AvoidanceByLCModule::updateSteeringFactorPtr(const BehaviorModuleOutput & output) -{ - const auto turn_signal_info = output.turn_signal_info; - const auto current_pose = getEgoPose(); - const double start_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status_.lane_change_path.shift_line.start.position); - const double finish_distance = motion_utils::calcSignedArcLength( - output.path->points, current_pose.position, status_.lane_change_path.shift_line.end.position); - - const uint16_t steering_factor_direction = - std::invoke([this, &start_distance, &finish_distance, &turn_signal_info]() { - if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - waitApprovalLeft(start_distance, finish_distance); - return SteeringFactor::LEFT; - } - if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - waitApprovalRight(start_distance, finish_distance); - return SteeringFactor::RIGHT; - } - return SteeringFactor::UNKNOWN; - }); - - // TODO(tkhmy) add handle status TRYING - steering_factor_interface_ptr_->updateSteeringFactor( - {status_.lane_change_path.shift_line.start, status_.lane_change_path.shift_line.end}, - {start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction, - SteeringFactor::TURNING, ""); -} - -void AvoidanceByLCModule::updateSteeringFactorPtr( - const CandidateOutput & output, const LaneChangePath & selected_path) const -{ - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - - steering_factor_interface_ptr_->updateSteeringFactor( - {selected_path.shift_line.start, selected_path.shift_line.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); -} - -std_msgs::msg::Header AvoidanceByLCModule::getRouteHeader() const -{ - return planner_data_->route_handler->getRouteHeader(); -} - -void AvoidanceByLCModule::generateExtendedDrivableArea(PathWithLaneId & path) -{ - const auto & common_parameters = planner_data_->parameters; - const auto & route_handler = planner_data_->route_handler; - const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, status_.current_lanes, status_.lane_change_lanes); - const auto target_drivable_lanes = getNonOverlappingExpandedLanes(path, drivable_lanes); - utils::generateDrivableArea( - path, target_drivable_lanes, common_parameters.vehicle_length, planner_data_); -} - -bool AvoidanceByLCModule::isApprovedPathSafe(Pose & ego_pose_before_collision) const -{ - const auto current_pose = getEgoPose(); - const auto current_twist = getEgoTwist(); - const auto & dynamic_objects = planner_data_->dynamic_object; - const auto & common_parameters = planner_data_->parameters; - const auto & lane_change_parameters = parameters_->lane_change; - const auto & route_handler = planner_data_->route_handler; - const auto & path = status_.lane_change_path; - - // get lanes used for detection - const auto backward_target_lanes_for_object_filtering = utils::lane_change::getBackwardLanelets( - *route_handler, path.target_lanelets, current_pose, check_distance_); - - std::unordered_map debug_data; - const auto lateral_buffer = - utils::lane_change::calcLateralBufferForFiltering(common_parameters.vehicle_width); - const auto dynamic_object_indices = utils::lane_change::filterObjectIndices( - {path}, *dynamic_objects, backward_target_lanes_for_object_filtering, current_pose, - common_parameters.forward_path_length, *lane_change_parameters, lateral_buffer); - - return utils::lane_change::isLaneChangePathSafe( - path, dynamic_objects, dynamic_object_indices, current_pose, current_twist, common_parameters, - *parameters_->lane_change, common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, ego_pose_before_collision, debug_data, - status_.lane_change_path.acceleration); -} - -void AvoidanceByLCModule::updateOutputTurnSignal(BehaviorModuleOutput & output) -{ - const auto turn_signal_info = utils::getPathTurnSignal( - status_.current_lanes, status_.lane_change_path.shifted_path, - status_.lane_change_path.shift_line, getEgoPose(), getEgoTwist().linear.x, - planner_data_->parameters); - output.turn_signal_info.turn_signal.command = turn_signal_info.first.command; - - utils::lane_change::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info); -} - -void AvoidanceByLCModule::resetParameters() -{ - is_abort_path_approved_ = false; - is_abort_approval_requested_ = false; - current_lane_change_state_ = LaneChangeStates::Normal; - abort_path_ = nullptr; - - object_debug_.clear(); - debug_marker_.markers.clear(); - resetPathCandidate(); - resetPathReference(); -} - -void AvoidanceByLCModule::acceptVisitor(const std::shared_ptr & visitor) const -{ - if (visitor) { - visitor->visitAvoidanceByLCModule(this); - } -} - -void SceneModuleVisitor::visitAvoidanceByLCModule( - [[maybe_unused]] const AvoidanceByLCModule * module) const -{ - // lane_change_visitor_ = module->get_debug_msg_array(); -} -} // namespace behavior_path_planner From 23f0a441c7f23c422c03ff2bb49a42369007a1de Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 8 May 2023 14:01:07 +0900 Subject: [PATCH 10/21] test(scenario_selector): add off-track test (#3592) Signed-off-by: Takamasa Horibe --- .../test_scenario_selector_node_interface.cpp | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp b/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp index 10c9d9863b7b7..d3d6939f49b83 100644 --- a/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp +++ b/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp @@ -31,6 +31,8 @@ std::shared_ptr generateTestManager() // set subscriber with topic name: scenario_selector → test_node_ test_manager->setScenarioSubscriber("output/scenario"); + test_manager->setOdometryTopicName("input/odometry"); + return test_manager; } @@ -99,3 +101,22 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectoryParkingMode) ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); rclcpp::shutdown(); } + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // set scenario_selector's input topic name(this topic is changed to test node) + test_manager->setTrajectoryInputTopicName("input/lane_driving/trajectory"); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + rclcpp::shutdown(); +} From 850775f5919524acfb1f8adb103245d1931cd759 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 8 May 2023 14:04:52 +0900 Subject: [PATCH 11/21] test(obstacle_cruise_planner): add off-track test (#3590) Signed-off-by: Takamasa Horibe --- ...obstacle_cruise_planner_node_interface.cpp | 60 ++++++++++++++++--- 1 file changed, 51 insertions(+), 9 deletions(-) diff --git a/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp b/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp index 5e35c710f7b1f..d15ea874a3a74 100644 --- a/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp +++ b/planning/obstacle_cruise_planner/test/test_obstacle_cruise_planner_node_interface.cpp @@ -21,12 +21,26 @@ #include -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +using motion_planning::ObstacleCruisePlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() { - rclcpp::init(0, nullptr); + auto test_manager = std::make_shared(); + + // set subscriber with topic name: obstacle_cruise_planner → test_node_ + test_manager->setTrajectorySubscriber("obstacle_cruise_planner/output/trajectory"); + + // set obstacle_cruise_planners input topic name(this topic is changed to test node): + test_manager->setTrajectoryInputTopicName("obstacle_cruise_planner/input/trajectory"); - auto test_manager = std::make_shared(); + test_manager->setOdometryTopicName("obstacle_cruise_planner/input/odometry"); + return test_manager; +} + +std::shared_ptr generateNode() +{ auto node_options = rclcpp::NodeOptions{}; const auto obstacle_cruise_planner_dir = ament_index_cpp::get_package_share_directory("obstacle_cruise_planner"); @@ -38,19 +52,27 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", obstacle_cruise_planner_dir + "/config/default_common.param.yaml", "--params-file", obstacle_cruise_planner_dir + "/config/obstacle_cruise_planner.param.yaml"}); - auto test_target_node = - std::make_shared(node_options); + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "obstacle_cruise_planner/input/odometry"); test_manager->publishPredictedObjects(test_target_node, "obstacle_cruise_planner/input/objects"); test_manager->publishAcceleration(test_target_node, "obstacle_cruise_planner/input/acceleration"); +} - // set subscriber with topic name: obstacle_cruise_planner → test_node_ - test_manager->setTrajectorySubscriber("obstacle_cruise_planner/output/trajectory"); +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); - // set obstacle_cruise_planners input topic name(this topic is changed to test node): - test_manager->setTrajectoryInputTopicName("obstacle_cruise_planner/input/trajectory"); + publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); @@ -58,4 +80,24 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) // test for trajectory with empty/one point/overlapping point ASSERT_NO_THROW(test_manager->testWithAbnormalTrajectory(test_target_node)); + + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW(test_manager->testWithNominalTrajectory(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test for trajectory with empty/one point/overlapping point + ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); } From c46ba350e2bc138c25b91b9d512cc959e6c57093 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 8 May 2023 15:15:47 +0900 Subject: [PATCH 12/21] fix(behavior_path_planner): terminate turn signal when it passes end point (#3631) Signed-off-by: yutaka --- .../src/turn_signal_decider.cpp | 27 ++++++++++++++++--- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 61723bee6743b..a69fdea549646 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -28,6 +28,16 @@ namespace behavior_path_planner { +double calc_distance( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const Pose & input_point, const double nearest_dist_threshold, const double nearest_yaw_threshold) +{ + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); +} + TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( const std::shared_ptr & planner_data, const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info) @@ -73,6 +83,16 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( if (!intersection_turn_signal_info) { initialize_intersection_info(); + const auto & desired_end_point = turn_signal_info.desired_end_point; + const double dist_to_end_point = calc_distance( + extended_path, current_pose, ego_seg_idx, desired_end_point, nearest_dist_threshold, + nearest_yaw_threshold); + if (dist_to_end_point < 0.0) { + TurnIndicatorsCommand updated_turn_signal; + updated_turn_signal.stamp = turn_signal_info.turn_signal.stamp; + updated_turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + return updated_turn_signal; + } return turn_signal_info.turn_signal; } else if ( turn_signal_info.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND || @@ -224,10 +244,9 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( const double nearest_dist_threshold, const double nearest_yaw_threshold) { const auto get_distance = [&](const Pose & input_point) { - const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); - return motion_utils::calcSignedArcLength( - path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); + return calc_distance( + path, current_pose, current_seg_idx, input_point, nearest_dist_threshold, + nearest_yaw_threshold); }; const auto & inter_desired_start_point = intersection_signal_info.desired_start_point; From 1356149701bd3bc96a150a6b7c81cb2d5a6f5250 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 8 May 2023 15:47:00 +0900 Subject: [PATCH 13/21] fix(intersection): fixed virtual wall marker id duplication (#3603) Signed-off-by: Mamoru Sobue --- .../src/scene_module/intersection/debug.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index a28b15493be56..fca5f73b608e2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -223,7 +223,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker &wall_marker, now); } - auto id = 0; + auto id = planning_utils::bitShift(module_id_); for (auto & marker : wall_marker.markers) { if (marker.action == visualization_msgs::msg::Marker::ADD) { marker.id = id; From bc41dbe695af645f5afc5b77df76d589088285dc Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 8 May 2023 16:17:41 +0900 Subject: [PATCH 14/21] feat: handle NaNs when using OSQP (#3490) Signed-off-by: Maxime CLEMENT --- .../src/qp_solver/qp_solver_osqp.cpp | 8 +++- .../src/smoother/jerk_filtered_smoother.cpp | 11 +++++ .../src/smoother/l2_pseudo_jerk_smoother.cpp | 11 +++++ .../smoother/linf_pseudo_jerk_smoother.cpp | 11 +++++ .../src/eb_path_smoother.cpp | 6 +++ .../src/mpt_optimizer.cpp | 7 +++ .../velocity_optimizer.cpp | 44 +++++++++++-------- 7 files changed, 78 insertions(+), 20 deletions(-) diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index 9bd4d4ab02769..dba803e8f5efc 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -60,8 +60,14 @@ bool QPSolverOSQP::solve( const int status_val = std::get<3>(result); if (status_val != 1) { - // TODO(Horibe): Should return false and the failure must be handled in an appropriate way. RCLCPP_WARN(logger_, "optimization failed : %s", osqpsolver_.getStatusMessage().c_str()); + return false; + } + const auto has_nan = + std::any_of(U_osqp.begin(), U_osqp.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; } // polish status: successful (1), unperformed (0), (-1) unsuccessful diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 8d46fc3f10a02..be5b0bf712fb2 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -294,6 +294,17 @@ bool JerkFilteredSmoother::apply( // execute optimization const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); const std::vector optval = std::get<0>(result); + const int status_val = std::get<3>(result); + if (status_val != 1) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + return false; + } + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; + } const auto tf1 = std::chrono::system_clock::now(); const double dt_ms1 = diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index d9b2375d96b28..d46deeeeb9f10 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -190,6 +190,17 @@ bool L2PseudoJerkSmoother::apply( // [b0, b1, ..., bN, | a0, a1, ..., aN, | // delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN] const std::vector optval = std::get<0>(result); + const int status_val = std::get<3>(result); + if (status_val != 1) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + return false; + } + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; + } for (unsigned int i = 0; i < N; ++i) { double v = optval.at(i); diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index 6381c1a8fdac8..e8900b1947454 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -205,6 +205,17 @@ bool LinfPseudoJerkSmoother::apply( // [b0, b1, ..., bN, | a0, a1, ..., aN, | // delta0, delta1, ..., deltaN, | sigma0, sigma1, ..., sigmaN] const std::vector optval = std::get<0>(result); + const int status_val = std::get<3>(result); + if (status_val != 1) { + RCLCPP_WARN(logger_, "optimization failed : %s", qp_solver_.getStatusMessage().c_str()); + return false; + } + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return false; + } /* get velocity & acceleration */ for (unsigned int i = 0; i < N; ++i) { diff --git a/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp index a988f78c9074e..31af863eed8f0 100644 --- a/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp +++ b/planning/obstacle_avoidance_planner/src/eb_path_smoother.cpp @@ -380,6 +380,12 @@ std::optional> EBPathSmoother::optimizeTrajectory() osqp_solver_ptr_->logUnsolvedStatus("[EB]"); return std::nullopt; } + const auto has_nan = std::any_of( + optimized_points.begin(), optimized_points.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return std::nullopt; + } time_keeper_ptr_->toc(__func__, " "); return optimized_points; diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 83295b61c42a1..5171dfddc88e6 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -1540,6 +1540,13 @@ std::optional MPTOptimizer::calcOptimizedSteerAngles( // get optimization result auto optimization_result = std::get<0>(result); // NOTE: const cannot be added due to the next operation. + const auto has_nan = std::any_of( + optimization_result.begin(), optimization_result.end(), + [](const auto v) { return std::isnan(v); }); + if (has_nan) { + RCLCPP_WARN(logger_, "optimization failed: result contains NaN values"); + return std::nullopt; + } const Eigen::VectorXd optimized_steer_angles = Eigen::Map(&optimization_result[0], D_un); diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp index ff96994a4a095..c8932da6f17e5 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp @@ -248,29 +248,35 @@ VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const Optimiza const std::vector optval = std::get<0>(result); const int status_val = std::get<3>(result); - if (status_val != 1) { + if (status_val != 1) std::cerr << "optimization failed : " << qp_solver_.getStatusMessage().c_str() << std::endl; - } - std::vector opt_time = time_vec; - std::vector opt_pos(N); - std::vector opt_vel(N); - std::vector opt_acc(N); - std::vector opt_jerk(N); - for (size_t i = 0; i < N; ++i) { - opt_pos.at(i) = optval.at(IDX_S0 + i); - opt_vel.at(i) = std::max(optval.at(IDX_V0 + i), 0.0); - opt_acc.at(i) = optval.at(IDX_A0 + i); - opt_jerk.at(i) = optval.at(IDX_J0 + i); - } - opt_vel.back() = 0.0; + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) std::cerr << "optimization failed : result contains NaN values\n"; OptimizationResult optimized_result; - optimized_result.t = opt_time; - optimized_result.s = opt_pos; - optimized_result.v = opt_vel; - optimized_result.a = opt_acc; - optimized_result.j = opt_jerk; + const auto is_optimization_failed = status_val != 1 || has_nan; + if (!is_optimization_failed) { + std::vector opt_time = time_vec; + std::vector opt_pos(N); + std::vector opt_vel(N); + std::vector opt_acc(N); + std::vector opt_jerk(N); + for (size_t i = 0; i < N; ++i) { + opt_pos.at(i) = optval.at(IDX_S0 + i); + opt_vel.at(i) = std::max(optval.at(IDX_V0 + i), 0.0); + opt_acc.at(i) = optval.at(IDX_A0 + i); + opt_jerk.at(i) = optval.at(IDX_J0 + i); + } + opt_vel.back() = 0.0; + + optimized_result.t = opt_time; + optimized_result.s = opt_pos; + optimized_result.v = opt_vel; + optimized_result.a = opt_acc; + optimized_result.j = opt_jerk; + } return optimized_result; } From ccdcb5c8ed8d93c48888ab31ad48e4addf300889 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 8 May 2023 16:45:35 +0900 Subject: [PATCH 15/21] refactor(avoidance): remove hard code params (#3624) * refactor(avoidance): remove hard code params in shift line triming process Signed-off-by: satoshi-ota * refactor(avoidance): update config Signed-off-by: satoshi-ota * chore(avoidance): add description for params Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 8 ++++ .../avoidance/avoidance_module.hpp | 6 +-- .../utils/avoidance/avoidance_module_data.hpp | 15 ++++++ .../src/behavior_path_planner_node.cpp | 14 ++++++ .../avoidance/avoidance_module.cpp | 48 +++++++++---------- .../src/scene_module/avoidance/manager.cpp | 14 ++++++ 6 files changed, 76 insertions(+), 29 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 0183c227bce9c..71a98a65baf89 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -143,3 +143,11 @@ matrix: [2.78, 13.9, # velocity [m/s] 0.50, 1.00] # margin [m] + + shift_line_pipeline: + trim: + quantize_filter_threshold: 0.2 + same_grad_filter_1_threshold: 0.1 + same_grad_filter_2_threshold: 0.2 + same_grad_filter_3_threshold: 0.5 + sharp_shift_filter_threshold: 0.2 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 3d0fbf72bdf9d..bd65d4a7b285b 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 @@ -228,12 +228,12 @@ class AvoidanceModule : public SceneModuleInterface // shift point generation: trimmers AvoidLineArray trimShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; - void quantizeShiftLine(AvoidLineArray & shift_lines, const double interval) const; - void trimSmallShiftLine(AvoidLineArray & shift_lines, const double shift_diff_thres) const; + void quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const; + void trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const; void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const; void trimMomentaryReturn(AvoidLineArray & shift_lines) const; void trimTooSharpShift(AvoidLineArray & shift_lines) const; - void trimSharpReturn(AvoidLineArray & shift_lines) const; + void trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const; // shift point generation: return-shift generator void addReturnShiftLineFromEgo( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index c1fc19db51658..e5c25081474f4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -238,6 +238,21 @@ struct AvoidanceParameters // avoidance path will be generated unless their lateral margin difference exceeds this value. double avoidance_execution_lateral_threshold; + // For shift line generation process. The continuous shift length is quantized by this value. + double quantize_filter_threshold; + + // For shift line generation process. Merge small shift lines. (First step) + double same_grad_filter_1_threshold; + + // For shift line generation process. Merge small shift lines. (Second step) + double same_grad_filter_2_threshold; + + // For shift line generation process. Merge small shift lines. (Third step) + double same_grad_filter_3_threshold; + + // For shift line generation process. Remove sharp(=jerky) shift line. + double sharp_shift_filter_threshold; + // target velocity matrix std::vector target_velocity_matrix; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index bb491086992f0..87edf64c97674 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -657,6 +657,20 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.target_velocity_matrix = declare_parameter>(ns + "matrix"); } + // shift line pipeline + { + std::string ns = "avoidance.shift_line_pipeline."; + p.quantize_filter_threshold = declare_parameter(ns + "trim.quantize_filter_threshold"); + p.same_grad_filter_1_threshold = + declare_parameter(ns + "trim.same_grad_filter_1_threshold"); + p.same_grad_filter_2_threshold = + declare_parameter(ns + "trim.same_grad_filter_2_threshold"); + p.same_grad_filter_3_threshold = + declare_parameter(ns + "trim.same_grad_filter_3_threshold"); + p.sharp_shift_filter_threshold = + declare_parameter(ns + "trim.sharp_shift_filter_threshold"); + } + return p; } 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 d028fc9fc3800..1ab26c99f59a2 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 @@ -1288,8 +1288,8 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Combine avoid points that have almost same gradient. // this is to remove the noise. { - const auto CHANGE_SHIFT_THRESHOLD_FOR_NOISE = 0.1; - trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD_FOR_NOISE); + const auto THRESHOLD = parameters_->same_grad_filter_1_threshold; + trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); debug.trim_similar_grad_shift = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift"); } @@ -1297,8 +1297,8 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Quantize the shift length to reduce the shift point noise // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc. { - constexpr double QUANTIZATION_DISTANCE = 0.2; - quantizeShiftLine(sl_array_trimmed, QUANTIZATION_DISTANCE); + const auto THRESHOLD = parameters_->quantize_filter_threshold; + quantizeShiftLine(sl_array_trimmed, THRESHOLD); printShiftLines(sl_array_trimmed, "after sl_array_trimmed"); debug.quantized = sl_array_trimmed; } @@ -1313,8 +1313,8 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Combine avoid points that have almost same gradient (again) { - const auto CHANGE_SHIFT_THRESHOLD = 0.2; - trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD); + const auto THRESHOLD = parameters_->same_grad_filter_2_threshold; + trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); debug.trim_similar_grad_shift_second = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); } @@ -1323,15 +1323,16 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // Check if it is not too sharp for the return-to-center shift point. // If the shift is sharp, it is combined with the next shift point until it gets non-sharp. { - trimSharpReturn(sl_array_trimmed); + const auto THRESHOLD = parameters_->sharp_shift_filter_threshold; + trimSharpReturn(sl_array_trimmed, THRESHOLD); debug.trim_too_sharp_shift = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trimSharpReturn"); } // - Combine avoid points that have almost same gradient (again) { - const auto CHANGE_SHIFT_THRESHOLD = 0.5; - trimSimilarGradShiftLine(sl_array_trimmed, CHANGE_SHIFT_THRESHOLD); + const auto THRESHOLD = parameters_->same_grad_filter_3_threshold; + trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); debug.trim_similar_grad_shift_third = sl_array_trimmed; printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); } @@ -1362,21 +1363,20 @@ void AvoidanceModule::alignShiftLinesOrder( } } -void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double interval) const +void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const { - if (interval < 1.0e-5) { + if (threshold < 1.0e-5) { return; // no need to process } for (auto & sl : shift_lines) { - sl.end_shift_length = std::round(sl.end_shift_length / interval) * interval; + sl.end_shift_length = std::round(sl.end_shift_length / threshold) * threshold; } alignShiftLinesOrder(shift_lines); } -void AvoidanceModule::trimSmallShiftLine( - AvoidLineArray & shift_lines, const double shift_diff_thres) const +void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const { AvoidLineArray shift_lines_orig = shift_lines; shift_lines.clear(); @@ -1391,7 +1391,7 @@ void AvoidanceModule::trimSmallShiftLine( auto sl_modified = sl_now; // remove the shift point if the length is almost same as the previous one. - if (std::abs(shift_diff) < shift_diff_thres) { + if (std::abs(shift_diff) < threshold) { sl_modified.end_shift_length = sl_prev.end_shift_length; sl_modified.start_shift_length = sl_prev.end_shift_length; DEBUG_PRINT( @@ -1409,7 +1409,7 @@ void AvoidanceModule::trimSmallShiftLine( } void AvoidanceModule::trimSimilarGradShiftLine( - AvoidLineArray & avoid_lines, const double change_shift_dist_threshold) const + AvoidLineArray & avoid_lines, const double threshold) const { AvoidLineArray avoid_lines_orig = avoid_lines; avoid_lines.clear(); @@ -1436,8 +1436,7 @@ void AvoidanceModule::trimSimilarGradShiftLine( const auto longitudinal = original.end_longitudinal - combined_al.start_longitudinal; const auto new_length = combined_al.getGradient() * longitudinal + combined_al.start_shift_length; - const bool has_large_change = - std::abs(new_length - original.end_shift_length) > change_shift_dist_threshold; + const bool has_large_change = std::abs(new_length - original.end_shift_length) > threshold; DEBUG_PRINT( "original.end_shift_length: %f, original.end_longitudinal: %f, " @@ -1446,7 +1445,7 @@ void AvoidanceModule::trimSimilarGradShiftLine( original.end_shift_length, original.end_longitudinal, combined_al.start_longitudinal, combined_al.getGradient(), new_length, has_large_change); - if (std::abs(new_length - original.end_shift_length) > change_shift_dist_threshold) { + if (std::abs(new_length - original.end_shift_length) > threshold) { return true; } } @@ -1620,7 +1619,7 @@ void AvoidanceModule::trimMomentaryReturn(AvoidLineArray & shift_lines) const DEBUG_PRINT("trimMomentaryReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size()); } -void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines) const +void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const { AvoidLineArray shift_lines_orig = shift_lines; shift_lines.clear(); @@ -1651,17 +1650,14 @@ void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines) const }; // Check if the merged shift has a conflict with the original shifts. - const auto hasViolation = [this](const auto & combined, const auto & combined_src) { - constexpr auto VIOLATION_SHIFT_THR = 0.3; + const auto hasViolation = [&threshold](const auto & combined, const auto & combined_src) { for (const auto & sl : combined_src) { const auto combined_shift = utils::avoidance::lerpShiftLengthOnArc(sl.end_longitudinal, combined); - if ( - sl.end_shift_length < -0.01 && combined_shift > sl.end_shift_length + VIOLATION_SHIFT_THR) { + if (sl.end_shift_length < -0.01 && combined_shift > sl.end_shift_length + threshold) { return true; } - if ( - sl.end_shift_length > 0.01 && combined_shift < sl.end_shift_length - VIOLATION_SHIFT_THR) { + if (sl.end_shift_length > 0.01 && combined_shift < sl.end_shift_length - threshold) { return true; } } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 993219ebfbb62..9f1b836d6e29e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -95,6 +95,20 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "max_lateral_jerk", p->max_lateral_jerk); } + { + const std::string ns = "avoidance.shift_line_pipeline."; + updateParam( + parameters, ns + "trim.quantize_filter_threshold", p->quantize_filter_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_1_threshold", p->same_grad_filter_1_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_2_threshold", p->same_grad_filter_2_threshold); + updateParam( + parameters, ns + "trim.same_grad_filter_3_threshold", p->same_grad_filter_3_threshold); + updateParam( + parameters, ns + "trim.sharp_shift_filter_threshold", p->sharp_shift_filter_threshold); + } + std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { m->updateModuleParams(p); }); From 52127f0e7849a15928db370b99b5e2dcfa5b5e52 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 8 May 2023 16:54:49 +0900 Subject: [PATCH 16/21] feat(behavior_path_planner): renew lane change collision check functions (#3609) * feat(behavior_path_planner): renew lane change collision check Signed-off-by: yutaka * update Signed-off-by: yutaka --------- Signed-off-by: yutaka --- .../utils/safety_check.hpp | 53 ++- .../src/utils/lane_change/utils.cpp | 11 +- .../src/utils/safety_check.cpp | 327 +++++++++--------- .../test/test_lane_change_utils.cpp | 7 +- 4 files changed, 198 insertions(+), 200 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp index b1ae41beeb229..7183d86521fd2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp @@ -42,6 +42,7 @@ namespace behavior_path_planner::utils::safety_check using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using marker_utils::CollisionCheckDebug; @@ -59,32 +60,6 @@ bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); -/** - * @brief Project nearest point on a line segment. - * @param [in] reference_point point to project - * @param [in] line segment - * @return nearest point on the line segment - */ -template > -ProjectedDistancePoint pointToSegment( - const Point2d & reference_point, const Point2d & polygon_segment_start, - const Point2d & polygon_segment_end); - -/** - * @brief Find nearest points between two polygon. - */ -void getProjectedDistancePointFromPolygons( - const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego, - Pose & point_on_object); - -/** - * @brief get relative pose with reference to the target object. - * @param [in] absolute pose desired_pose reference pose - * @param [in] absolute pose target_pose target pose to check - * @return relative pose of the target - */ -Pose projectCurrentPoseToTarget(const Pose & reference_pose, const Pose & target_pose); - /** * @brief find which vehicle is front and rear and check for lateral, * longitudinal physical and longitudinal expected stopping distance between two points @@ -99,10 +74,22 @@ Pose projectCurrentPoseToTarget(const Pose & reference_pose, const Pose & target * @return true if distance is safe. */ bool hasEnoughDistance( - const Pose & expected_ego_pose, const Twist & ego_current_twist, - const Pose & expected_object_pose, const Twist & object_current_twist, - const BehaviorPathPlannerParameters & param, const double front_decel, const double rear_decel, - CollisionCheckDebug & debug); + const Polygon2d & front_object_polygon, const double front_object_velocity, + const Polygon2d & rear_object_polygon, const double rear_object_velocity, + const bool is_object_front, const BehaviorPathPlannerParameters & param, + const double front_object_deceleration, const double rear_object_deceleration); + +void getTransformedPolygon( + const Pose & front_object_pose, const Pose & rear_object_pose, + const vehicle_info_util::VehicleInfo & ego_vehicle_info, const Shape & object_shape, + const bool is_object_front, Polygon2d & transformed_front_object_polygon, + Polygon2d & transformed_rear_object_polygon); + +double calcLateralDistance( + const Polygon2d & front_object_polygon, const Polygon2d & rear_object_polygon); + +double calcLongitudinalDistance( + const Polygon2d & front_object_polygon, const Polygon2d & rear_object_polygon); /** * @brief Iterate the points in the ego and target's predicted path and @@ -110,6 +97,7 @@ bool hasEnoughDistance( * @return true if distance is safe. */ bool isSafeInLaneletCollisionCheck( + const PathWithLaneId & path, const std::vector> & interpolated_ego, const Twist & ego_current_twist, const std::vector & check_duration, const double prepare_duration, const PredictedObject & target_object, @@ -123,12 +111,13 @@ bool isSafeInLaneletCollisionCheck( * @return true if distance is safe. */ bool isSafeInFreeSpaceCollisionCheck( + const PathWithLaneId & path, const std::vector> & interpolated_ego, const Twist & ego_current_twist, const std::vector & check_duration, const double prepare_duration, const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, - const double prepare_phase_ignore_target_speed_thresh, const double front_decel, - const double rear_decel, CollisionCheckDebug & debug); + const double prepare_phase_ignore_target_velocity_thresh, const double front_object_deceleration, + const double rear_object_deceleration, CollisionCheckDebug & debug); } // namespace behavior_path_planner::utils::safety_check diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index aa4868c02792e..65f4b33c74291 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -326,8 +326,8 @@ bool isLaneChangePathSafe( utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { if (!utils::safety_check::isSafeInLaneletCollisionCheck( - interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, - obj, obj_path, common_parameter, + path, interpolated_ego, current_twist, check_durations, + lane_change_path.duration.prepare, obj, obj_path, common_parameter, lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel, rear_decel, ego_pose_before_collision, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); @@ -350,9 +350,10 @@ bool isLaneChangePathSafe( utils::getPredictedPathFromObj(obj, lane_change_parameter.use_all_predicted_path); if (!utils::safety_check::isSafeInFreeSpaceCollisionCheck( - interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, obj, - common_parameter, lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, - front_decel, rear_decel, current_debug_data.second)) { + path, interpolated_ego, current_twist, check_durations, lane_change_path.duration.prepare, + obj, common_parameter, + lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel, + rear_decel, current_debug_data.second)) { appendDebugInfo(current_debug_data, false); return false; } diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp index 1e5f9b8c52fe2..03b39937775c4 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/safety_check.cpp @@ -38,127 +38,34 @@ bool isTargetObjectFront( return false; } -template -ProjectedDistancePoint pointToSegment( - const Point2d & reference_point, const Point2d & polygon_segment_start, - const Point2d & polygon_segment_end) -{ - auto segment_vec = polygon_segment_end; - auto segment_start_to_reference_vec = reference_point; - bg::subtract_point(segment_vec, polygon_segment_start); - bg::subtract_point(segment_start_to_reference_vec, polygon_segment_start); - - const auto c1 = bg::dot_product(segment_vec, segment_start_to_reference_vec); - const auto c2 = bg::dot_product(segment_vec, segment_vec); - const auto ratio = std::clamp(c1 / c2, 0.0, 1.0); - - Point2d projected = polygon_segment_start; - bg::multiply_value(segment_vec, ratio); - bg::add_point(projected, segment_vec); - - return {projected, Pythagoras::apply(reference_point, projected)}; -} - -void getProjectedDistancePointFromPolygons( - const Polygon2d & ego_polygon, const Polygon2d & object_polygon, Pose & point_on_ego, - Pose & point_on_object) -{ - ProjectedDistancePoint nearest; - std::unique_ptr current_point; - - bool points_in_ego{false}; - - const auto findPoints = [&nearest, ¤t_point, &points_in_ego]( - const Polygon2d & polygon_for_segment, - const Polygon2d & polygon_for_points, const bool & ego_is_points) { - const auto segments = boost::make_iterator_range( - bg::segments_begin(polygon_for_segment), bg::segments_end(polygon_for_segment)); - const auto points = boost::make_iterator_range( - bg::points_begin(polygon_for_points), bg::points_end(polygon_for_points)); - - for (auto && segment : segments) { - for (auto && point : points) { - const auto projected = pointToSegment(point, *segment.first, *segment.second); - if (!current_point || projected.distance < nearest.distance) { - current_point = std::make_unique(point); - nearest = projected; - points_in_ego = ego_is_points; - } - } - } - }; - - std::invoke(findPoints, ego_polygon, object_polygon, false); - std::invoke(findPoints, object_polygon, ego_polygon, true); - - if (!points_in_ego) { - point_on_object.position.x = current_point->x(); - point_on_object.position.y = current_point->y(); - point_on_ego.position.x = nearest.projected_point.x(); - point_on_ego.position.y = nearest.projected_point.y(); - } else { - point_on_ego.position.x = current_point->x(); - point_on_ego.position.y = current_point->y(); - point_on_object.position.x = nearest.projected_point.x(); - point_on_object.position.y = nearest.projected_point.y(); - } -} -Pose projectCurrentPoseToTarget(const Pose & desired_object, const Pose & target_object) -{ - tf2::Transform tf_map_desired_to_global{}; - tf2::Transform tf_map_target_to_global{}; - - tf2::fromMsg(desired_object, tf_map_desired_to_global); - tf2::fromMsg(target_object, tf_map_target_to_global); - - Pose desired_obj_pose_projected_to_target{}; - tf2::toMsg( - tf_map_desired_to_global.inverse() * tf_map_target_to_global, - desired_obj_pose_projected_to_target); - - return desired_obj_pose_projected_to_target; -} - bool hasEnoughDistance( - const Pose & expected_ego_pose, const Twist & ego_current_twist, - const Pose & expected_object_pose, const Twist & object_current_twist, - const BehaviorPathPlannerParameters & param, const double front_decel, const double rear_decel, - CollisionCheckDebug & debug) + const Polygon2d & front_object_polygon, const double front_object_velocity, + const Polygon2d & rear_object_polygon, const double rear_object_velocity, + const bool is_object_front, const BehaviorPathPlannerParameters & param, + const double front_object_deceleration, const double rear_object_deceleration) { - const auto front_vehicle_pose = - projectCurrentPoseToTarget(expected_ego_pose, expected_object_pose); - debug.relative_to_ego = front_vehicle_pose; - // 1. Check lateral distance between ego and target object - if (std::abs(front_vehicle_pose.position.y) > param.lateral_distance_max_threshold) { + const double lat_dist = calcLateralDistance(front_object_polygon, rear_object_polygon); + if (lat_dist > param.lateral_distance_max_threshold) { return true; } - const auto is_obj_in_front = front_vehicle_pose.position.x > -1e-3; - debug.is_front = is_obj_in_front; - - const auto [front_vehicle_velocity, rear_vehicle_velocity] = std::invoke([&]() { - debug.object_twist.linear = object_current_twist.linear; - if (is_obj_in_front) { - return std::make_pair(object_current_twist.linear.x, ego_current_twist.linear.x); - } - return std::make_pair(ego_current_twist.linear.x, object_current_twist.linear.x); - }); - // 2. Check physical distance between ego and target object + const double lon_dist = calcLongitudinalDistance(front_object_polygon, rear_object_polygon); const auto is_unsafe_dist_between_vehicle = std::invoke([&]() { // ignore this for parked vehicle. - if (object_current_twist.linear.x < 0.1) { + const double object_velocity = is_object_front ? front_object_velocity : rear_object_velocity; + if (object_velocity < 0.1) { return false; } // the value guarantee distance between vehicles are always more than dist - const auto max_vel = std::max(front_vehicle_velocity, rear_vehicle_velocity); + const auto max_vel = std::max(front_object_velocity, rear_object_velocity); constexpr auto scale = 0.8; const auto dist = scale * std::abs(max_vel) + param.longitudinal_distance_min_threshold; // return value rounded to the nearest two floating point - return std::abs(front_vehicle_pose.position.x) < dist; + return lon_dist < dist; }); if (is_unsafe_dist_between_vehicle) { @@ -173,59 +80,133 @@ bool hasEnoughDistance( }; const auto front_vehicle_stop_threshold = - stoppingDistance(front_vehicle_velocity, front_decel) + std::abs(front_vehicle_pose.position.x); + stoppingDistance(front_object_velocity, front_object_deceleration) + lon_dist; // longitudinal_distance_min_threshold here guarantee future stopping distance must be more than // longitudinal_distance_min_threshold const auto rear_vehicle_stop_threshold = std::invoke([&]() { - const auto reaction_buffer = rear_vehicle_velocity * param.rear_vehicle_reaction_time; - const auto safety_buffer = rear_vehicle_velocity * param.rear_vehicle_safety_time_margin; + const auto reaction_buffer = rear_object_velocity * param.rear_vehicle_reaction_time; + const auto safety_buffer = rear_object_velocity * param.rear_vehicle_safety_time_margin; return std::max( - reaction_buffer + safety_buffer + stoppingDistance(rear_vehicle_velocity, rear_decel), + reaction_buffer + safety_buffer + + stoppingDistance(rear_object_velocity, rear_object_deceleration), param.longitudinal_distance_min_threshold); }); return rear_vehicle_stop_threshold <= front_vehicle_stop_threshold; } +void getTransformedPolygon( + const Pose & front_object_pose, const Pose & rear_object_pose, + const vehicle_info_util::VehicleInfo & ego_vehicle_info, const Shape & object_shape, + const bool is_object_front, Polygon2d & transformed_front_object_polygon, + Polygon2d & transformed_rear_object_polygon) +{ + // transform from map to base_link based on rear pose + const auto transformed_front_pose = + tier4_autoware_utils::inverseTransformPose(front_object_pose, rear_object_pose); + Pose transformed_rear_pose; + transformed_rear_pose.position = tier4_autoware_utils::createPoint(0.0, 0.0, 0.0); + transformed_rear_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); + + if (is_object_front) { + transformed_front_object_polygon = + tier4_autoware_utils::toPolygon2d(transformed_front_pose, object_shape); + transformed_rear_object_polygon = tier4_autoware_utils::toFootprint( + transformed_rear_pose, ego_vehicle_info.max_longitudinal_offset_m, + ego_vehicle_info.rear_overhang_m, ego_vehicle_info.vehicle_width_m); + } else { + transformed_front_object_polygon = tier4_autoware_utils::toFootprint( + transformed_front_pose, ego_vehicle_info.max_longitudinal_offset_m, + ego_vehicle_info.rear_overhang_m, ego_vehicle_info.vehicle_width_m); + transformed_rear_object_polygon = + tier4_autoware_utils::toPolygon2d(transformed_rear_pose, object_shape); + } + + return; +} + +double calcLateralDistance( + const Polygon2d & front_object_polygon, const Polygon2d & rear_object_polygon) +{ + double min_dist = 0.0; + for (const auto & rear_point : rear_object_polygon.outer()) { + double max_lateral_dist = std::numeric_limits::min(); + double min_lateral_dist = std::numeric_limits::max(); + for (const auto & front_point : front_object_polygon.outer()) { + const double lat_dist = front_point.y() - rear_point.y(); + max_lateral_dist = std::max(max_lateral_dist, lat_dist); + min_lateral_dist = std::min(min_lateral_dist, lat_dist); + } + + // sort lateral distance + if (max_lateral_dist * min_lateral_dist < 0) { + // if the sign is different, it means the object is crossing + return 0.0; + } + + const double dist = std::min(std::abs(max_lateral_dist), std::abs(min_lateral_dist)); + min_dist = std::min(min_dist, dist); + } + + return min_dist; +} + +double calcLongitudinalDistance( + const Polygon2d & front_object_polygon, const Polygon2d & rear_object_polygon) +{ + double min_dist = std::numeric_limits::max(); + for (const auto & rear_point : rear_object_polygon.outer()) { + double min_lon_dist = std::numeric_limits::max(); + for (const auto & front_point : front_object_polygon.outer()) { + const double lon_dist = front_point.x() - rear_point.x(); + if (lon_dist < 0.0) { + return 0.0; + } + min_lon_dist = std::min(lon_dist, min_lon_dist); + } + + min_dist = std::min(min_dist, min_lon_dist); + } + + return min_dist; +} + bool isSafeInLaneletCollisionCheck( + const PathWithLaneId & path, const std::vector> & interpolated_ego, const Twist & ego_current_twist, const std::vector & check_duration, const double prepare_duration, const PredictedObject & target_object, const PredictedPath & target_object_path, const BehaviorPathPlannerParameters & common_parameters, - const double prepare_phase_ignore_target_speed_thresh, const double front_decel, - const double rear_decel, Pose & ego_pose_before_collision, CollisionCheckDebug & debug) + const double prepare_phase_ignore_target_velocity_thresh, const double front_object_deceleration, + const double rear_object_deceleration, Pose & ego_pose_before_collision, + CollisionCheckDebug & debug) { debug.lerped_path.reserve(check_duration.size()); - const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; - const auto object_speed = object_twist.linear.x; - const auto ignore_check_at_time = [&](const double current_time) { - return ( - (current_time < prepare_duration) && - (object_speed < prepare_phase_ignore_target_speed_thresh)); - }; + const auto & ego_velocity = ego_current_twist.linear.x; + const auto & object_velocity = + target_object.kinematics.initial_twist_with_covariance.twist.linear.x; for (size_t i = 0; i < check_duration.size(); ++i) { const auto current_time = check_duration.at(i); - if (ignore_check_at_time(current_time)) { + if ( + current_time < prepare_duration && + object_velocity < prepare_phase_ignore_target_velocity_thresh) { continue; } - auto expected_obj_pose = - perception_utils::calcInterpolatedPose(target_object_path, current_time); - - if (!expected_obj_pose) { + const auto obj_pose = perception_utils::calcInterpolatedPose(target_object_path, current_time); + if (!obj_pose) { continue; } - const auto & obj_polygon = - tier4_autoware_utils::toPolygon2d(*expected_obj_pose, target_object.shape); - const auto & ego_info = interpolated_ego.at(i); - auto expected_ego_pose = ego_info.first; - const auto & ego_polygon = ego_info.second; + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, target_object.shape); + const auto & ego_pose = interpolated_ego.at(i).first; + const auto & ego_polygon = interpolated_ego.at(i).second; + // check overlap debug.ego_polygon = ego_polygon; debug.obj_polygon = obj_polygon; if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { @@ -233,49 +214,66 @@ bool isSafeInLaneletCollisionCheck( return false; } - debug.lerped_path.push_back(expected_ego_pose); - - getProjectedDistancePointFromPolygons( - ego_polygon, obj_polygon, expected_ego_pose, *expected_obj_pose); - debug.expected_ego_pose = expected_ego_pose; - debug.expected_obj_pose = *expected_obj_pose; + // compute which one is at the front of the other + const bool is_object_front = + isTargetObjectFront(path, ego_pose, common_parameters.vehicle_info, obj_polygon); + const auto & [front_object_pose, rear_object_pose] = + is_object_front ? std::make_pair(*obj_pose, ego_pose) : std::make_pair(ego_pose, *obj_pose); + const auto & [front_object_velocity, rear_object_velocity] = + is_object_front ? std::make_pair(object_velocity, ego_velocity) + : std::make_pair(ego_velocity, object_velocity); + + // get front and rear object polygon + Polygon2d front_object_polygon; + Polygon2d rear_object_polygon; + getTransformedPolygon( + front_object_pose, rear_object_pose, common_parameters.vehicle_info, target_object.shape, + is_object_front, front_object_polygon, rear_object_polygon); + + debug.lerped_path.push_back(ego_pose); + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = *obj_pose; + debug.relative_to_ego = front_object_pose; + debug.is_front = is_object_front; if (!hasEnoughDistance( - expected_ego_pose, ego_current_twist, *expected_obj_pose, object_twist, common_parameters, - front_decel, rear_decel, debug)) { + front_object_polygon, front_object_velocity, rear_object_polygon, rear_object_velocity, + is_object_front, common_parameters, front_object_deceleration, + rear_object_deceleration)) { debug.failed_reason = "not_enough_longitudinal"; return false; } - ego_pose_before_collision = expected_ego_pose; + ego_pose_before_collision = ego_pose; } return true; } bool isSafeInFreeSpaceCollisionCheck( + const PathWithLaneId & path, const std::vector> & interpolated_ego, const Twist & ego_current_twist, const std::vector & check_duration, const double prepare_duration, const PredictedObject & target_object, const BehaviorPathPlannerParameters & common_parameters, - const double prepare_phase_ignore_target_speed_thresh, const double front_decel, - const double rear_decel, CollisionCheckDebug & debug) + const double prepare_phase_ignore_target_velocity_thresh, const double front_object_deceleration, + const double rear_object_deceleration, CollisionCheckDebug & debug) { + const auto & obj_pose = target_object.kinematics.initial_pose_with_covariance.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(target_object); - const auto & object_twist = target_object.kinematics.initial_twist_with_covariance.twist; - const auto object_speed = object_twist.linear.x; - const auto ignore_check_at_time = [&](const double current_time) { - return ( - (current_time < prepare_duration) && - (object_speed < prepare_phase_ignore_target_speed_thresh)); - }; + const auto & object_velocity = + target_object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto & ego_velocity = ego_current_twist.linear.x; + for (size_t i = 0; i < check_duration.size(); ++i) { const auto current_time = check_duration.at(i); - if (ignore_check_at_time(current_time)) { + if ( + current_time < prepare_duration && + object_velocity < prepare_phase_ignore_target_velocity_thresh) { continue; } - const auto & ego_info = interpolated_ego.at(i); - auto expected_ego_pose = ego_info.first; - const auto & ego_polygon = ego_info.second; + + const auto & ego_pose = interpolated_ego.at(i).first; + const auto & ego_polygon = interpolated_ego.at(i).second; debug.ego_polygon = ego_polygon; debug.obj_polygon = obj_polygon; @@ -284,18 +282,29 @@ bool isSafeInFreeSpaceCollisionCheck( return false; } - auto expected_obj_pose = target_object.kinematics.initial_pose_with_covariance.pose; - getProjectedDistancePointFromPolygons( - ego_polygon, obj_polygon, expected_ego_pose, expected_obj_pose); - - debug.expected_ego_pose = expected_ego_pose; - debug.expected_obj_pose = expected_obj_pose; + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + + // compute which one is at the front of the other + const bool is_object_front = + isTargetObjectFront(path, ego_pose, common_parameters.vehicle_info, obj_polygon); + const auto & [front_object_pose, rear_object_pose] = + is_object_front ? std::make_pair(obj_pose, ego_pose) : std::make_pair(ego_pose, obj_pose); + const auto & [front_object_velocity, rear_object_velocity] = + is_object_front ? std::make_pair(object_velocity, ego_velocity) + : std::make_pair(ego_velocity, object_velocity); + + // get front and rear object polygon + Polygon2d front_object_polygon; + Polygon2d rear_object_polygon; + getTransformedPolygon( + front_object_pose, rear_object_pose, common_parameters.vehicle_info, target_object.shape, + is_object_front, front_object_polygon, rear_object_polygon); - const auto object_twist = target_object.kinematics.initial_twist_with_covariance.twist; if (!hasEnoughDistance( - expected_ego_pose, ego_current_twist, - target_object.kinematics.initial_pose_with_covariance.pose, object_twist, - common_parameters, front_decel, rear_decel, debug)) { + front_object_polygon, front_object_velocity, rear_object_polygon, rear_object_velocity, + is_object_front, common_parameters, front_object_deceleration, + rear_object_deceleration)) { debug.failed_reason = "not_enough_longitudinal"; return false; } diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index c3ca024a80ba6..a489af173c345 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -27,12 +27,11 @@ TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) ego_pose.position = tier4_autoware_utils::createPoint(0, 0, 0); geometry_msgs::msg::Pose obj_pose; - obj_pose.position = tier4_autoware_utils::createPoint(-4, 3, 0); const auto obj_yaw = tier4_autoware_utils::deg2rad(0.0); - ego_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(obj_yaw); + obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(obj_yaw); + obj_pose.position = tier4_autoware_utils::createPoint(-4, 3, 0); - const auto result = - behavior_path_planner::utils::safety_check::projectCurrentPoseToTarget(ego_pose, obj_pose); + const auto result = tier4_autoware_utils::inverseTransformPose(obj_pose, ego_pose); EXPECT_NEAR(result.position.x, -4, epsilon); EXPECT_NEAR(result.position.y, 3, epsilon); From a89b26116142906b6c43b53705c5ce7c5c6b6155 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 8 May 2023 18:45:23 +0900 Subject: [PATCH 17/21] feat(behavior_path_planner): disable safety check (#3633) Signed-off-by: yutaka --- .../behavior_path_planner/src/utils/lane_change/utils.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 65f4b33c74291..4dc5de2fdecd3 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -183,8 +183,13 @@ std::optional constructCandidatePath( point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; } + // TODO(Yutaka Shimizu): remove this flag after make the isPathInLanelets faster + const bool enable_path_check_in_lanelet = false; + // check candidate path is in lanelet - if (!isPathInLanelets(shifted_path.path, original_lanelets, target_lanelets)) { + if ( + enable_path_check_in_lanelet && + !isPathInLanelets(shifted_path.path, original_lanelets, target_lanelets)) { return std::nullopt; } From 4864b1901c67097bf4b692365af7d8766b42ef43 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 8 May 2023 19:52:29 +0900 Subject: [PATCH 18/21] feat(behavior_path_planner): hold previous drivable area info in LC (#3635) Signed-off-by: Takayuki Murooka --- .../scene_module/lane_change/base_class.hpp | 10 +++++----- .../src/scene_module/lane_change/interface.cpp | 3 +-- .../src/scene_module/lane_change/normal.cpp | 2 +- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 011431f5017b3..8d63414a4d1b8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -59,7 +59,7 @@ class LaneChangeBase { prev_module_reference_path_ = std::make_shared(); prev_module_path_ = std::make_shared(); - prev_drivable_lanes_ = std::make_shared>(); + prev_drivable_area_info_ = std::make_shared(); } LaneChangeBase(const LaneChangeBase &) = delete; @@ -100,10 +100,10 @@ class LaneChangeBase } }; - virtual void setPreviousDrivableLanes(const std::vector & prev_drivable_lanes) + virtual void setPreviousDrivableAreaInfo(const DrivableAreaInfo & prev_drivable_area_info) { - if (prev_drivable_lanes_) { - *prev_drivable_lanes_ = prev_drivable_lanes; + if (prev_drivable_area_info_) { + *prev_drivable_area_info_ = prev_drivable_area_info; } } @@ -231,7 +231,7 @@ class LaneChangeBase std::shared_ptr planner_data_{}; std::shared_ptr prev_module_reference_path_{}; std::shared_ptr prev_module_path_{}; - std::shared_ptr> prev_drivable_lanes_{}; + std::shared_ptr prev_drivable_area_info_{}; PathWithLaneId prev_approved_path_{}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 9889a94648942..2e3dfe7545ca7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -130,8 +130,7 @@ BehaviorModuleOutput LaneChangeInterface::plan() resetPathIfAbort(); } - module_type_->setPreviousDrivableLanes( - getPreviousModuleOutput().drivable_area_info.drivable_lanes); + module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); auto output = module_type_->generateOutput(); path_reference_ = output.reference_path; *prev_approved_path_ = *getPreviousModuleOutput().path; 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 419a02470ff8b..c5d1f0eb25bdd 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 @@ -521,7 +521,7 @@ std::vector NormalLaneChange::getDrivableLanes() const { const auto drivable_lanes = utils::lane_change::generateDrivableLanes( *getRouteHandler(), status_.current_lanes, status_.lane_change_lanes); - return utils::combineDrivableLanes(*prev_drivable_lanes_, drivable_lanes); + return utils::combineDrivableLanes(prev_drivable_area_info_->drivable_lanes, drivable_lanes); } bool NormalLaneChange::isApprovedPathSafe(Pose & ego_pose_before_collision) const From 9693e36365e8dd1269eef88976174d3600a684c6 Mon Sep 17 00:00:00 2001 From: Narendar Selvaraj <48509412+Narendarselva@users.noreply.github.com> Date: Mon, 8 May 2023 17:29:51 +0530 Subject: [PATCH 19/21] feat(freespace_planning_algorithms): fix performance issue in freespace planning with respect to collision table initialization using A-Search algo (#2621) * Performance issue fix with respect to collision table initialization * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kosuke Takeuchi --- .../abstract_algorithm.hpp | 4 ++++ .../src/abstract_algorithm.cpp | 13 ++++++++----- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp index 690750ac0ab5b..b4e3b2befae40 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp @@ -120,6 +120,7 @@ class AbstractPlanningAlgorithm const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape) : planner_common_param_(planner_common_param), collision_vehicle_shape_(collision_vehicle_shape) { + is_collision_table_initialized = false; } AbstractPlanningAlgorithm( @@ -179,6 +180,9 @@ class AbstractPlanningAlgorithm geometry_msgs::msg::Pose start_pose_; geometry_msgs::msg::Pose goal_pose_; + // Is collision table initalized + bool is_collision_table_initialized; + // result path PlannerWaypoints waypoints_; }; diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index 2b5814ee2a111..159c0df82fcc3 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -123,11 +123,14 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost is_obstacle_table_ = is_obstacle_table; // construct collision indexes table - for (int i = 0; i < planner_common_param_.theta_size; i++) { - std::vector indexes_2d, vertex_indexes_2d; - computeCollisionIndexes(i, indexes_2d, vertex_indexes_2d); - coll_indexes_table_.push_back(indexes_2d); - vertex_indexes_table_.push_back(vertex_indexes_2d); + if (is_collision_table_initialized == false) { + for (int i = 0; i < planner_common_param_.theta_size; i++) { + std::vector indexes_2d, vertex_indexes_2d; + computeCollisionIndexes(i, indexes_2d, vertex_indexes_2d); + coll_indexes_table_.push_back(indexes_2d); + vertex_indexes_table_.push_back(vertex_indexes_2d); + } + is_collision_table_initialized = true; } } From 568baab59057f86255d9ea210439f8f3fb4bebcf Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 9 May 2023 08:02:05 +0900 Subject: [PATCH 20/21] docs(behavior): update planner manager document (#3614) * docs(behavior): add code link Signed-off-by: satoshi-ota * docs(behavior): add figure Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_planner_interface_design.md | 4 + .../behavior_path_planner_manager_design.md | 167 ++++++-- .../image/manager/avoidance.svg | 80 ++++ .../image/manager/example_behavior.svg | 397 ++++++++++++++++++ .../image/manager/lane_change.svg | 212 ++++++++++ .../image/manager/module_select.svg | 168 ++++++++ .../image/manager/multiple_candidates.svg | 248 +++++++++++ .../image/manager/output_module.svg | 226 ++++++++++ .../image/manager/request_step1.svg | 386 +++++++++++++++++ .../image/manager/request_step2-2.svg | 229 ++++++++++ .../image/manager/request_step2-3.svg | 232 ++++++++++ .../image/manager/request_step2.svg | 231 ++++++++++ .../image/manager/request_step3.svg | 126 ++++++ .../image/manager/request_step4-2.svg | 244 +++++++++++ .../image/manager/request_step4-3.svg | 245 +++++++++++ .../image/manager/request_step4.svg | 239 +++++++++++ .../image/manager/request_step5.svg | 183 ++++++++ .../image/manager/root_generation.svg | 93 ++++ .../image/manager/root_lanelet.svg | 166 ++++++++ 19 files changed, 3851 insertions(+), 25 deletions(-) create mode 100644 planning/behavior_path_planner/image/manager/avoidance.svg create mode 100644 planning/behavior_path_planner/image/manager/example_behavior.svg create mode 100644 planning/behavior_path_planner/image/manager/lane_change.svg create mode 100644 planning/behavior_path_planner/image/manager/module_select.svg create mode 100644 planning/behavior_path_planner/image/manager/multiple_candidates.svg create mode 100644 planning/behavior_path_planner/image/manager/output_module.svg create mode 100644 planning/behavior_path_planner/image/manager/request_step1.svg create mode 100644 planning/behavior_path_planner/image/manager/request_step2-2.svg create mode 100644 planning/behavior_path_planner/image/manager/request_step2-3.svg create mode 100644 planning/behavior_path_planner/image/manager/request_step2.svg create mode 100644 planning/behavior_path_planner/image/manager/request_step3.svg create mode 100644 planning/behavior_path_planner/image/manager/request_step4-2.svg create mode 100644 planning/behavior_path_planner/image/manager/request_step4-3.svg create mode 100644 planning/behavior_path_planner/image/manager/request_step4.svg create mode 100644 planning/behavior_path_planner/image/manager/request_step5.svg create mode 100644 planning/behavior_path_planner/image/manager/root_generation.svg create mode 100644 planning/behavior_path_planner/image/manager/root_lanelet.svg diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md index 12a7576493040..4072e09f5df3a 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md @@ -1 +1,5 @@ # Interface design + +!!! warning + + Under Construction diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md index 27d1449568b77..9e11f0b7eaf5a 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -4,9 +4,13 @@ The manager launches and executes scene modules in `behavior_path_planner` depending on the use case, and has been developed to achieve following features: -- Multiple modules can run simultaneously in series in order to achieve more complex use cases. +- Multiple modules can run simultaneously in series in order to achieve more complex use cases. For example, as shown in the following video, this manager make it possible to avoid a parked vehicle during lane change maneuver. - Flexible development by not relying on framework from external libraries. +![example](../image/manager/example_behavior.svg) + +[Movie](https://user-images.githubusercontent.com/44889564/231639251-6631dc4e-1861-47e7-9c51-6df1b666ce9b.mp4) + Support status: | Name | Simple exclusive execution | Advanced simultaneous execution | @@ -51,7 +55,10 @@ The sub-manager's main task is - delete expired scene module instance from `registered_modules_`. - publish debug markers. -![sub_managers](../image/manager/sub_managers.svg) +
+ ![sub_managers](../image/manager/sub_managers.svg){width=1000} +
sub-managers
+
Sub-manager is registered on the manager with the following function. @@ -68,6 +75,8 @@ void registerSceneModuleManager(const SceneModuleManagerPtr & manager_ptr) } ``` +Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp#L66-L75) + Sub-manager has the following parameters that are needed by the manager to manage the launched modules, and these parameters can be set for each module. ```c++ @@ -81,6 +90,8 @@ struct ModuleConfigParameters }; ``` +Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp#L23-L30) + | Name | Type | Description | | :-------------------------------------------------- | :------ | :--------------------------------------------------------------------------------------------------------------------- | | `enable_module` | bool | if true, the sub-manager is registered on the manager. | @@ -93,7 +104,10 @@ struct ModuleConfigParameters Scene modules receives necessary data and RTC command, and outputs candidate path(s), reference path and RTC cooperate status. When multiple modules run in series, the output of the previous module is received as input and the information is used to generate a new modified path, as shown in the following figure. And, when one module is running alone, it receives a reference path generated from the centerline of the lane in which Ego is currently driving as previous module output. -![scene_module](../image/manager/scene_module.svg) +
+ ![scene_module](../image/manager/scene_module.svg){width=1000} +
scene module
+
| I/O | Type | Description | | :-- | :-------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | @@ -115,30 +129,44 @@ Scene modules running on the manager are stored on the **candidate modules stack ## Process flow -There are 5 steps in one process: +There are 6 steps in one process: -**Step1.** At first, the manager set latest planner data, and run all approved modules and get output path. At this time, the manager checks module status and removes expired modules from approved modules stack. +### Step1 + +At first, the manager set latest planner data, and run all approved modules and get output path. At this time, the manager checks module status and removes expired modules from approved modules stack. ![process_step1](../image/manager/process_step1.svg) -**Step2.** Input approved modules output and necessary data to all registered modules, and the modules judge the necessity of path modification based on it. The manager checks which module makes execution request. +### Step2 + +Input approved modules output and necessary data to all registered modules, and the modules judge the necessity of path modification based on it. The manager checks which module makes execution request. ![process_step2](../image/manager/process_step2.svg) -**Step3.** Check request module existence. +### Step3 + +Check request module existence. -**Step4.** The manager decides which module to execute as candidate modules from the modules that requested to execute path modification. +### Step4 + +The manager decides which module to execute as candidate modules from the modules that requested to execute path modification. ![process_step4](../image/manager/process_step4.svg) -**Step5.** Decides the priority order of execution among candidate modules. And, run all candidate modules. Each modules outputs reference path and RTC cooperate status. +### Step5 + +Decides the priority order of execution among candidate modules. And, run all candidate modules. Each modules outputs reference path and RTC cooperate status. ![process_step5](../image/manager/process_step5.svg) -**Step6.** Move approved module to approved modules stack from candidate modules stack. +### Step6 + +Move approved module to approved modules stack from candidate modules stack. ![process_step6](../image/manager/process_step6.svg) +--- + and, within a single planning cycle, these steps are repeated until the following conditions are satisfied. - **Any modules don't make a request of path modification. (Check in Step3)** @@ -259,6 +287,8 @@ detach } ``` +Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/src/planner_manager.cpp#L66-L111) + ## Priority of execution request Compare priorities parameter among sub-managers to determine the order of execution based on config. Therefore, the priority between sub-modules does **NOT** change at runtime. @@ -278,15 +308,23 @@ Compare priorities parameter among sub-managers to determine the order of execut } ``` +Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp#L239-L250) + In the future, however, we are considering having the priorities change dynamically depending on the situation in order to achieve more complex use cases. ## How to decide which request modules to run? On this manager, it is possible that multiple scene modules may request path modification at same time. In that case, the modules to be executed as candidate module is determined in the following order. -**Step1.** Push back the modules that make a request to `request_modules`. +### Step1 -**Step2.** Check approved modules stack, and remove non-executable modules from`request_modules` based on the following condition. +Push back the modules that make a request to `request_modules`. + +![request_step1](../image/manager/request_step1.svg) + +### Step2 + +Check approved modules stack, and remove non-executable modules from`request_modules` based on the following condition. - **Condition A.** approved module stack is empty. - **Condition B.** all modules in approved modules stack support simultaneous execution as approved module (`enable_simultaneous_execution_as_approved_module` is `true`). @@ -336,9 +374,26 @@ Executable or not: If a module that doesn't support simultaneous execution exists in approved modules stack (**NOT** satisfy Condition B), no more modules can be added to the stack, and therefore none of the modules can be executed as candidate. -**Step3.** Sort `request_modules` by priority. +For example, if approved module's setting of `enable_simultaneous_execution_as_approved_module` is **ENABLE**, then only modules whose the setting is **ENABLE** proceed to the next step. + +![request_step2](../image/manager/request_step2.svg) + +Other examples: + +| Process | Description | +| :------------------------------------------------------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ![request_step2-2](../image/manager/request_step2-2.svg) | If approved modules stack is empty, then all request modules proceed to the next step, regardless of the setting of `enable_simultaneous_execution_as_approved_module`. | +| ![request_step2-3](../image/manager/request_step2-3.svg) | If approved module's setting of `enable_simultaneous_execution_as_approved_module` is **DISABLE**, then all request modules are discarded. | -**Step4.** Check and pick up executable modules as candidate in order of priority based on the following conditions. +### Step3 + +Sort `request_modules` by priority. + +![request_step3](../image/manager/request_step3.svg) + +### Step4 + +Check and pick up executable modules as candidate in order of priority based on the following conditions. - **Condition A.** candidate module stack is empty. - **Condition B.** all modules in candidate modules stack support simultaneous execution as candidate module (`enable_simultaneous_execution_as_candidate_module` is `true`). @@ -386,13 +441,30 @@ Executable or not: | NO | NO | YES | NO | | NO | NO | NO | NO | -**Step5.** Run all candidate modules. +For example, if the highest priority module's setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE**, then all modules after the second priority are discarded. + +![request_step4](../image/manager/request_step4.svg) + +Other examples: + +| Process | Description | +| :------------------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ![request_step4-2](../image/manager/request_step4-2.svg) | If a module with a higher priority exists, lower priority modules whose setting of `enable_simultaneous_execution_as_candidate_module` is **DISABLE** are discarded. | +| ![request_step4-3](../image/manager/request_step4-3.svg) | If all modules' setting of `enable_simultaneous_execution_as_candidate_module` is **ENABLE**, then all modules proceed to the next step. | + +### Step5 + +Run all candidate modules. + +![request_step5](../image/manager/request_step5.svg) ## How to decide which module's output to use? -Additionally, the manager selects a candidate modules which output path is used as `behavior_path_planner` output by approval condition in the following order. +Sometimes, multiple candidate modules are running simultaneously. -**Step1.** Check all candidate modules' approval condition, and sort the priority based on the following rules. +![multiple_candidates](../image/manager/multiple_candidates.svg) + +In this case, the manager selects a candidate modules which output path is used as `behavior_path_planner` output by approval condition in the following rules. - **Rule A.** Regardless of the priority in the sub-manager (`priority`), approved modules always have a higher priority than unapproved modules. - **Rule B.** If the approval status is the same, sort according to the sub-manager's priority. @@ -408,9 +480,14 @@ Additionally, the manager selects a candidate modules which output path is used The smaller the number is, the higher the priority is. -**Step2.** Select the highest priority module. +
+ ![module_select](../image/manager/module_select.svg){width=1000} +
module priority
+
+ +![output_module](../image/manager/output_module.svg) -**Step3.** Move the highest priority module to approved modules stack if it is already approved. +Additionally, the manager moves the highest priority module to approved modules stack if it is already approved. ## Scene module unregister process @@ -452,11 +529,31 @@ When the manager removes succeeded modules, the last added module's output is us ## Reference path generation -The manager generates root reference path from the centerline of the ego driving lanes. The root reference path is not only used as an input to the first added module of approved modules stack, but also used as the output of `behavior_path_planner` if none of the modules are running. +The root reference path is generated from the centerline of the **lanelet sequence** that obtained from the **root lanelet**, and it is not only used as an input to the first added module of approved modules stack, but also used as the output of `behavior_path_planner` if none of the modules are running. + +
+ ![root_generation](../image/manager/root_generation.svg){width=500} +
root reference path generation
+
+ +The root lanelet is the closest lanelet within the route, and the update timing is based on Ego's operation mode state. + +- the state is `OperationModeState::AUTONOMOUS`: Update only when the ego moves to right or left lane by lane change module. +- the state is **NOT** `OperationModeState::AUTONOMOUS`: Update at the beginning of every planning cycle. + +![root_lanelet](../image/manager/root_lanelet.svg) + +The manager needs to know the ego behavior and then generate a root reference path from the lanes that Ego should follow. + +For example, during autonomous driving, even if Ego moves into the next lane in order to avoid a parked vehicle, the target lanes that Ego should follow will **NOT** change because Ego will return to the original lane after the avoidance maneuver. Therefore, the manager does **NOT** update **root lanelet** even if the avoidance maneuver is finished. + +![avoidance](../image/manager/avoidance.svg) -Additionally, the manager samples Ego's closest lanelet when removing a succeeded **Lane Change** module from approved modules stack. +On the other hand, if the lane change is successful, the manager updates **root lanelet** because the lane that Ego should follow changes. -This closest lanelet is used for root reference path generation. +![lane_change](../image/manager/lane_change.svg) + +In addition, while manual driving, the manager always updates **root lanelet** because the pilot may move to an adjacent lane regardless of the decision of the autonomous driving system. ```c++ /** @@ -478,10 +575,30 @@ This closest lanelet is used for root reference path generation. root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); lanelet::ConstLanelet closest_lane{}; - if (!lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { - return {}; + if (lanelet::utils::query::getClosestLaneletWithConstrains( + lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold)) { + return utils::getReferencePath(closest_lane, data); + } + + if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { + return utils::getReferencePath(closest_lane, data); } - return utils::getReferencePath(closest_lane, data); + return {}; // something wrong. } ``` + +Code is [here](https://github.com/autowarefoundation/autoware.universe/blob/b1734916e3efd9786507a271e0fe829dd37476c8/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp#L202-L227) + +## Drivable area generation + +!!! warning + + Under Construction + +## Turn signal management + +!!! warning + + Under Construction diff --git a/planning/behavior_path_planner/image/manager/avoidance.svg b/planning/behavior_path_planner/image/manager/avoidance.svg new file mode 100644 index 0000000000000..b61231aa69b17 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/avoidance.svg @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + +
+
+
+ ego's driving lane +
+
+
+ + ego's driving lane + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/example_behavior.svg b/planning/behavior_path_planner/image/manager/example_behavior.svg new file mode 100644 index 0000000000000..5213154714a2d --- /dev/null +++ b/planning/behavior_path_planner/image/manager/example_behavior.svg @@ -0,0 +1,397 @@ + + + + + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + +
+
+
+

+ Execute avoidance module during lane change maneuver. +

+
+
+
+ + Execute avoidance module during lane change maneuver. + + + + + + + + + +
+
+
+ output path +
+
+
+ + output p... + + + + + + +
+
+
+ candidate path +
+
+
+ + candidat... + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + + + + + + + +
+
+
+ want to move next lane... +
+
+
+ + want to move nex... + + + + + + + + + + + +
+
+
+ want to avoid the obstacle... +
+
+
+ + want to avoid th... + + + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + + +
+
+
+ found obstacle on lane change target lane... +
+
+
+ + found obstacle o... + + + + + + +
+
+
+ avoidance module starts running. +
+
+
+ + avoidance module starts running. + + + + + + +
+
+
+ lane change module is still running. +
+
+
+ + lane change module is still running. + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/lane_change.svg b/planning/behavior_path_planner/image/manager/lane_change.svg new file mode 100644 index 0000000000000..b0a01d6b9daae --- /dev/null +++ b/planning/behavior_path_planner/image/manager/lane_change.svg @@ -0,0 +1,212 @@ + + + + + + + + + + + + + + +
+
+
+ ego's driving lane +
+
+
+ + ego's driving lane + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + + +
+
+
+ before lane change +
+
+
+ + before lane change + + + + + + +
+
+
+ after lane change +
+
+
+ + after lane change + + + + + + + +
+
+
+ root lanelet +
+
+
+ + root lanelet + + + + + + + +
+
+
+ root lanelet (UPDATED) +
+
+
+ + root lanelet (UPDATED) + + + + + + + +
+
+
+ lanelet +
+
+
+ + lanelet + + + + + + +
+
+
+ lanelet +
+
+
+ + lanelet + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/module_select.svg b/planning/behavior_path_planner/image/manager/module_select.svg new file mode 100644 index 0000000000000..5033a54e480e6 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/module_select.svg @@ -0,0 +1,168 @@ + + + + + + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B + +
+ + condition: + APPROVED +
+ priority: + + 1 +
+ +
+
+
+ + scene module Bcondition: APPROVED... + + + + + + +
+
+
+ scene module D +
+ condition: WAITING APPROVAL +
+ priority: + 1 +
+
+
+ + scene module D... + + + + + + + +
+
+
+ priority: HIGH +
+
+
+ + priority: HIGH + + + + + + +
+
+
+ priority: LOW +
+
+
+ + priority: LOW + + + + + + +
+
+
+ scene module C + +
+ + condition: + APPROVED +
+ + priority: 99 +
+ +
+
+
+ + scene module Ccondition: APPROVED... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/multiple_candidates.svg b/planning/behavior_path_planner/image/manager/multiple_candidates.svg new file mode 100644 index 0000000000000..650b0b76548d1 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/multiple_candidates.svg @@ -0,0 +1,248 @@ + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + + + + + + + +
+
+
+ approved module output +
+
+
+ + approved module output + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/output_module.svg b/planning/behavior_path_planner/image/manager/output_module.svg new file mode 100644 index 0000000000000..ec3079114f4ea --- /dev/null +++ b/planning/behavior_path_planner/image/manager/output_module.svg @@ -0,0 +1,226 @@ + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + + + + + + + + + + +
+
+
+ approved module output +
+
+
+ + approved module output + + + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + +
+
+
+ Highest priority +
+
+
+ + Highest priority + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step1.svg b/planning/behavior_path_planner/image/manager/request_step1.svg new file mode 100644 index 0000000000000..a3e3489f0cb7f --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step1.svg @@ -0,0 +1,386 @@ + + + + + + + + + +
+
+
+ manager +
+
+
+ + manager + + + + + + +
+
+
+ sub-managers +
+
+
+ + sub-managers + + + + + + +
+
+
+ sub-manager A +
+
+
+ + sub-manager A + + + + + + +
+
+
+ sub-manager B +
+
+
+ + sub-manager B + + + + + + +
+
+
+ sub-manager C +
+
+
+ + sub-manager C + + + + + + +
+
+
+ sub-manager D +
+
+
+ + sub-manager D + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ Execution request!!! +
+
+
+ + Execution request!!! + + + + + + +
+
+
+ Execution request!!! +
+
+
+ + Execution request!!! + + + + + + +
+
+
+ Execution request!!! +
+
+
+ + Execution request!!! + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + +
+
+
+ scene module C +
+
+
+ + scene module C + + + + + + +
+
+
+ scene module D +
+
+
+ + scene module D + + + + + + +
+
+
+ launch +
+
+
+ + launch + + + + + + +
+
+
+ launch +
+
+
+ + launch + + + + + + +
+
+
+ launch +
+
+
+ + launch + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step2-2.svg b/planning/behavior_path_planner/image/manager/request_step2-2.svg new file mode 100644 index 0000000000000..f93ae15728181 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step2-2.svg @@ -0,0 +1,229 @@ + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module B... + + + + + + +
+
+
+ scene module C +
+ simultaneous execution as approved: + DISABLE +
+
+
+ + scene module C... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ EMPTY +
+
+
+ + EMPTY + + + + + + + + + +
+
+
+ To next step +
+
+
+ + To next step + + + + + + +
+
+
+ To next step +
+
+
+ + To next step + + + + + + +
+
+
+ To next step +
+
+
+ + To next step + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step2-3.svg b/planning/behavior_path_planner/image/manager/request_step2-3.svg new file mode 100644 index 0000000000000..38ef0df67f3f2 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step2-3.svg @@ -0,0 +1,232 @@ + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module B... + + + + + + +
+
+
+ scene module C +
+ simultaneous execution as approved: + DISABLE +
+
+
+ + scene module C... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+ simultaneous execution as approved: + DISABLE +
+
+
+ + scene module A... + + + + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step2.svg b/planning/behavior_path_planner/image/manager/request_step2.svg new file mode 100644 index 0000000000000..b51d1a62cd35d --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step2.svg @@ -0,0 +1,231 @@ + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module B... + + + + + + +
+
+
+ scene module C +
+ simultaneous execution as approved: + DISABLE +
+
+
+ + scene module C... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+ simultaneous execution as approved: ENABLE +
+
+
+ + scene module A... + + + + + + + + + +
+
+
+ To next step +
+
+
+ + To next step + + + + + + +
+
+
+ To next step +
+
+
+ + To next step + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step3.svg b/planning/behavior_path_planner/image/manager/request_step3.svg new file mode 100644 index 0000000000000..631cf38245b17 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step3.svg @@ -0,0 +1,126 @@ + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B +
+ priority: + HIGHEST +
+
+
+ + scene module B... + + + + + + +
+
+
+ scene module D +
+ priority: LOWEST +
+
+
+ + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step4-2.svg b/planning/behavior_path_planner/image/manager/request_step4-2.svg new file mode 100644 index 0000000000000..d35c8c60ec78f --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step4-2.svg @@ -0,0 +1,244 @@ + + + + + + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B + +
+ + simultaneous execution as candidate: ENABLE + +
+ +
+
+
+ + scene module Bsimultaneous execution as... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as candidate: + DISABLE +
+
+
+ + scene module D... + + + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + + +
+
+
+ priority: HIGH +
+
+
+ + priority: HIGH + + + + + + +
+
+
+ priority: LOW +
+
+
+ + priority: LOW + + + + + + +
+
+
+ scene module C + +
+ + simultaneous execution as candidate: ENABLE + +
+ +
+
+
+ + scene module Csimultaneous execution as... + + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step4-3.svg b/planning/behavior_path_planner/image/manager/request_step4-3.svg new file mode 100644 index 0000000000000..78240c9f5a307 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step4-3.svg @@ -0,0 +1,245 @@ + + + + + + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B + +
+ + simultaneous execution as candidate: ENABLE + +
+ +
+
+
+ + scene module Bsimultaneous execution as... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as candidate: ENABLE +
+
+
+ + scene module D... + + + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + + +
+
+
+ priority: HIGH +
+
+
+ + priority: HIGH + + + + + + +
+
+
+ priority: LOW +
+
+
+ + priority: LOW + + + + + + +
+
+
+ scene module C + +
+ + simultaneous execution as candidate: ENABLE + +
+ +
+
+
+ + scene module Csimultaneous execution as... + + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step4.svg b/planning/behavior_path_planner/image/manager/request_step4.svg new file mode 100644 index 0000000000000..c4b1d8d385122 --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step4.svg @@ -0,0 +1,239 @@ + + + + + + + + + + + + + + +
+
+
+ request modules +
+
+
+ + request modules + + + + + + +
+
+
+ scene module B + +
+ + simultaneous execution as candidate: + + DISABLE +
+ +
+
+
+ + scene module Bsimultaneous execution as... + + + + + + +
+
+
+ scene module D +
+ simultaneous execution as candidate: ENABLE +
+
+
+ + scene module D... + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + + + +
+
+
+ Push back +
+ candidate modules stack +
+
+
+ + Push back... + + + + + + +
+
+
+ Discard +
+
+
+ + Discard + + + + + + + +
+
+
+ priority: HIGH +
+
+
+ + priority: HIGH + + + + + + +
+
+
+ priority: LOW +
+
+
+ + priority: LOW + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/request_step5.svg b/planning/behavior_path_planner/image/manager/request_step5.svg new file mode 100644 index 0000000000000..54b5b7840995b --- /dev/null +++ b/planning/behavior_path_planner/image/manager/request_step5.svg @@ -0,0 +1,183 @@ + + + + + + + + + +
+
+
+ approved modules stack +
+
+
+ + approved modules stack + + + + + + +
+
+
+ candidate modules stack +
+
+
+ + candidate modules stack + + + + + + +
+
+
+ scene module A +
+
+
+ + scene module A + + + + + + +
+
+
+ scene module B +
+
+
+ + scene module B + + + + + + + + + +
+
+
+ approved module output +
+
+
+ + approved module output + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + +
+
+
+ output +
+
+
+ + output + + + + + + + + + + +
+
+
+ executing +
+
+
+ + executing + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/root_generation.svg b/planning/behavior_path_planner/image/manager/root_generation.svg new file mode 100644 index 0000000000000..a944e5c348f2c --- /dev/null +++ b/planning/behavior_path_planner/image/manager/root_generation.svg @@ -0,0 +1,93 @@ + + + + + + + + + + +
+
+
+ root lanelet +
+
+
+ + root lanelet + + + + + + + +
+
+
+ driving lane data +
+
+
+ + driving lane data + + + + + + + +
+
+
+ root reference path +
+
+
+ + root reference path + + + + + + +
+
+
+ generate root reference path +
+
+
+ + generate root refere... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/manager/root_lanelet.svg b/planning/behavior_path_planner/image/manager/root_lanelet.svg new file mode 100644 index 0000000000000..802eb1613e77b --- /dev/null +++ b/planning/behavior_path_planner/image/manager/root_lanelet.svg @@ -0,0 +1,166 @@ + + + + + + + + + + +
+
+
+ root lanelet +
+
+
+ + root lanelet + + + + + + +
+
+
+ lanelet +
+
+
+ + lanelet + + + + + + +
+
+
+ lanelet +
+
+
+ + lanelet + + + + + + +
+
+
+ lanelet +
+
+
+ + lanelet + + + + + + + +
+
+
+ root reference path +
+
+
+ + root refe... + + + + + + + + +
+
+
+ Ego +
+
+
+ + Ego + + + + + + + +
+
+
+ lanelet sequence +
+
+
+ + lanelet sequence + + + + + + + + + + Text is not SVG - cannot display + + + From ce1255a2e82f47c642edcb48ed5764c4431c89c0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 9 May 2023 08:31:18 +0900 Subject: [PATCH 21/21] feat(behavior_path_planner): resolve multiple modules turn signal info (#3634) feat(behavior_path_planner): resolve multiple modules turn signal Signed-off-by: satoshi-ota --- .../behavior_path_planner_node.hpp | 6 +- .../behavior_path_planner/data_manager.hpp | 30 +++--- .../turn_signal_decider.hpp | 30 +++++- .../src/behavior_path_planner_node.cpp | 13 +-- .../src/turn_signal_decider.cpp | 98 ++++++++++++++++--- 5 files changed, 131 insertions(+), 46 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 2c1ef1038554e..29ed73b9374e6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -37,7 +37,6 @@ #endif #include "behavior_path_planner/steering_factor_interface.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" @@ -136,8 +135,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool has_received_map_{false}; bool has_received_route_{false}; - TurnSignalDecider turn_signal_decider_; - std::mutex mutex_pd_; // mutex for planner_data_ std::mutex mutex_manager_; // mutex for bt_manager_ or planner_manager_ std::mutex mutex_map_; // mutex for has_received_map_ and map_ptr_ @@ -220,7 +217,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node /** * @brief publish steering factor from intersection */ - void publish_steering_factor(const TurnIndicatorsCommand & turn_signal); + void publish_steering_factor( + const std::shared_ptr & planner_data, const TurnIndicatorsCommand & turn_signal); /** * @brief publish left and right bound diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 41981f17b4f9e..f48e82a9e2af6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__DATA_MANAGER_HPP_ #include "behavior_path_planner/parameters.hpp" +#include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include @@ -96,24 +97,6 @@ struct DrivableAreaInfo bool is_already_expanded{false}; }; -struct TurnSignalInfo -{ - TurnSignalInfo() - { - turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; - hazard_signal.command = HazardLightsCommand::NO_COMMAND; - } - - // desired turn signal - TurnIndicatorsCommand turn_signal; - HazardLightsCommand hazard_signal; - - geometry_msgs::msg::Pose desired_start_point; - geometry_msgs::msg::Pose desired_end_point; - geometry_msgs::msg::Pose required_start_point; - geometry_msgs::msg::Pose required_end_point; -}; - struct BehaviorModuleOutput { BehaviorModuleOutput() = default; @@ -160,6 +143,17 @@ struct PlannerData BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; + mutable TurnSignalDecider turn_signal_decider; + + TurnIndicatorsCommand getTurnSignal( + const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info) + { + const auto & current_pose = self_odometry->pose.pose; + const auto & current_vel = self_odometry->twist.twist.linear.x; + return turn_signal_decider.getTurnSignal( + route_handler, path, turn_signal_info, current_pose, current_vel, parameters); + } + template size_t findEgoIndex(const std::vector & points) const { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp index f702fe1b4c8e3..5691c03d31f6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_ #define BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_ -#include +#include #include #include @@ -45,18 +45,42 @@ const std::map signal_map = { {"straight", TurnIndicatorsCommand::DISABLE}, {"none", TurnIndicatorsCommand::DISABLE}}; +struct TurnSignalInfo +{ + TurnSignalInfo() + { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + hazard_signal.command = HazardLightsCommand::NO_COMMAND; + } + + // desired turn signal + TurnIndicatorsCommand turn_signal; + HazardLightsCommand hazard_signal; + + geometry_msgs::msg::Pose desired_start_point; + geometry_msgs::msg::Pose desired_end_point; + geometry_msgs::msg::Pose required_start_point; + geometry_msgs::msg::Pose required_end_point; +}; + class TurnSignalDecider { public: TurnIndicatorsCommand getTurnSignal( - const std::shared_ptr & planner_data, const PathWithLaneId & path, - const TurnSignalInfo & turn_signal_info); + const std::shared_ptr & route_handler, const PathWithLaneId & path, + const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, + const BehaviorPathPlannerParameters & parameters); TurnIndicatorsCommand resolve_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info, const double nearest_dist_threshold, const double nearest_yaw_threshold); + TurnSignalInfo use_prior_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold); + void setParameters( const double base_link2front, const double intersection_search_distance, const double intersection_search_time, const double intersection_angle_threshold_deg) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 87edf64c97674..7adb135da3f61 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -313,7 +313,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const double turn_signal_intersection_angle_threshold_deg = planner_data_->parameters.turn_signal_intersection_angle_threshold_deg; const double turn_signal_search_time = planner_data_->parameters.turn_signal_search_time; - turn_signal_decider_.setParameters( + planner_data_->turn_signal_decider.setParameters( planner_data_->parameters.base_link2front, turn_signal_intersection_search_distance, turn_signal_search_time, turn_signal_intersection_angle_threshold_deg); } @@ -1249,7 +1249,7 @@ void BehaviorPathPlannerNode::computeTurnSignal( turn_signal.command = TurnIndicatorsCommand::DISABLE; hazard_signal.command = output.turn_signal_info.hazard_signal.command; } else { - turn_signal = turn_signal_decider_.getTurnSignal(planner_data, path, output.turn_signal_info); + turn_signal = planner_data->getTurnSignal(path, output.turn_signal_info); hazard_signal.command = HazardLightsCommand::DISABLE; } turn_signal.stamp = get_clock()->now(); @@ -1257,13 +1257,14 @@ void BehaviorPathPlannerNode::computeTurnSignal( turn_signal_publisher_->publish(turn_signal); hazard_signal_publisher_->publish(hazard_signal); - publish_steering_factor(turn_signal); + publish_steering_factor(planner_data, turn_signal); } -void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsCommand & turn_signal) +void BehaviorPathPlannerNode::publish_steering_factor( + const std::shared_ptr & planner_data, const TurnIndicatorsCommand & turn_signal) { const auto [intersection_flag, approaching_intersection_flag] = - turn_signal_decider_.getIntersectionTurnSignalFlag(); + planner_data->turn_signal_decider.getIntersectionTurnSignalFlag(); if (intersection_flag || approaching_intersection_flag) { const uint16_t steering_factor_direction = std::invoke([&turn_signal]() { if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -1273,7 +1274,7 @@ void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsComman }); const auto [intersection_pose, intersection_distance] = - turn_signal_decider_.getIntersectionPoseAndDistance(); + planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); const uint16_t steering_factor_state = std::invoke([&intersection_flag]() { if (intersection_flag) { return SteeringFactor::TURNING; diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index a69fdea549646..9234f06bf897d 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -39,34 +39,29 @@ double calc_distance( } TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( - const std::shared_ptr & planner_data, const PathWithLaneId & path, - const TurnSignalInfo & turn_signal_info) + const std::shared_ptr & route_handler, const PathWithLaneId & path, + const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, + const BehaviorPathPlannerParameters & parameters) { // Guard if (path.points.empty()) { return turn_signal_info.turn_signal; } - // Data - const double nearest_dist_threshold = planner_data->parameters.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = planner_data->parameters.ego_nearest_yaw_threshold; - const auto & current_pose = planner_data->self_odometry->pose.pose; - const double & current_vel = planner_data->self_odometry->twist.twist.linear.x; - const auto route_handler = *(planner_data->route_handler); - // Get current lanelets - const double forward_length = planner_data->parameters.forward_path_length; + const double forward_length = parameters.forward_path_length; + const double nearest_dist_threshold = parameters.ego_nearest_dist_threshold; + const double nearest_yaw_threshold = parameters.ego_nearest_yaw_threshold; const double backward_length = 50.0; - const lanelet::ConstLanelets current_lanes = utils::calcLaneAroundPose( - planner_data->route_handler, current_pose, forward_length, backward_length); + const lanelet::ConstLanelets current_lanes = + utils::calcLaneAroundPose(route_handler, current_pose, forward_length, backward_length); if (current_lanes.empty()) { return turn_signal_info.turn_signal; } const PathWithLaneId extended_path = utils::getCenterLinePath( - route_handler, current_lanes, current_pose, backward_length, forward_length, - planner_data->parameters); + *route_handler, current_lanes, current_pose, backward_length, forward_length, parameters); if (extended_path.points.empty()) { return turn_signal_info.turn_signal; @@ -78,7 +73,7 @@ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( // Get closest intersection turn signal if exists const auto intersection_turn_signal_info = getIntersectionTurnSignalInfo( - extended_path, current_pose, current_vel, ego_seg_idx, route_handler, nearest_dist_threshold, + extended_path, current_pose, current_vel, ego_seg_idx, *route_handler, nearest_dist_threshold, nearest_yaw_threshold); if (!intersection_turn_signal_info) { @@ -333,6 +328,79 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( return intersection_signal_info.turn_signal; } +TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) +{ + const auto get_distance = [&](const Pose & input_point) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); + }; + + const auto & original_desired_start_point = original_signal.desired_start_point; + const auto & original_desired_end_point = original_signal.desired_end_point; + const auto & original_required_start_point = original_signal.required_start_point; + const auto & original_required_end_point = original_signal.required_end_point; + const auto & new_desired_start_point = new_signal.desired_start_point; + const auto & new_desired_end_point = new_signal.desired_end_point; + const auto & new_required_start_point = new_signal.required_start_point; + const auto & new_required_end_point = new_signal.required_end_point; + + const double dist_to_original_desired_start = + get_distance(original_desired_start_point) - base_link2front_; + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_original_required_start = + get_distance(original_required_start_point) - base_link2front_; + const double dist_to_original_required_end = get_distance(original_required_end_point); + const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_; + const double dist_to_new_desired_end = get_distance(new_desired_end_point); + const double dist_to_new_required_start = + get_distance(new_required_start_point) - base_link2front_; + const double dist_to_new_required_end = get_distance(new_required_end_point); + + // If we still do not reach the desired front point we ignore it + if (dist_to_original_desired_start > 0.0 && dist_to_new_desired_start > 0.0) { + TurnSignalInfo empty_signal_info; + return empty_signal_info; + } else if (dist_to_original_desired_start > 0.0) { + return new_signal; + } else if (dist_to_new_desired_start > 0.0) { + return original_signal; + } + + // If we already passed the desired end point, return the other signal + if (dist_to_original_desired_end < 0.0 && dist_to_new_desired_end < 0.0) { + TurnSignalInfo empty_signal_info; + return empty_signal_info; + } else if (dist_to_original_desired_end < 0.0) { + return new_signal; + } else if (dist_to_new_desired_end < 0.0) { + return original_signal; + } + + if (dist_to_original_desired_start <= dist_to_new_desired_start) { + const auto enable_prior = use_prior_turn_signal( + dist_to_original_required_start, dist_to_original_required_end, dist_to_new_required_start, + dist_to_new_required_end); + + if (enable_prior) { + return original_signal; + } + return new_signal; + } + + const auto enable_prior = use_prior_turn_signal( + dist_to_new_required_start, dist_to_new_required_end, dist_to_original_required_start, + dist_to_original_required_end); + if (enable_prior) { + return new_signal; + } + return original_signal; +} + bool TurnSignalDecider::use_prior_turn_signal( const double dist_to_prior_required_start, const double dist_to_prior_required_end, const double dist_to_subsequent_required_start, const double dist_to_subsequent_required_end)