From f537e9cb7fa1706059d6c9d5faea82060601d1c0 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 31 Aug 2023 12:00:06 +0900 Subject: [PATCH 1/2] fix(walkway): fix module launch condition (#4823) Signed-off-by: satoshi-ota --- .../src/manager.cpp | 47 ++++++++----------- 1 file changed, 19 insertions(+), 28 deletions(-) diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index d10ff52850752..f9c222f662e82 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -44,11 +44,8 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - if (!opt_use_regulatory_element_) { - opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); - } - const auto launch = [this, &path](const auto & lanelet) { + const auto launch = [this, &path](const auto & lanelet, const auto use_regulatory_element) { const auto attribute = lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")); if (attribute != lanelet::AttributeValueString::Walkway) { @@ -64,24 +61,21 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - lanelet.id(), lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_)); + lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); }; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - launch(crosswalk.first->crosswalkLanelet()); - } - } else { - const auto crosswalk_lanelets = getCrosswalksOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + for (const auto & crosswalk : crosswalk_leg_elem_map) { + launch(crosswalk.first->crosswalkLanelet(), true); + } - for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk); - } + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk, false); } } @@ -92,17 +86,14 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) std::set walkway_id_set; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + walkway_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - walkway_id_set.insert(crosswalk.first->id()); - } - } else { - walkway_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + walkway_id_set.insert(crosswalk.first->id()); } return [walkway_id_set](const std::shared_ptr & scene_module) { From 6c52d513b85316eb7f80eca1672f9b773f789a2f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 4 Sep 2023 10:08:11 +0900 Subject: [PATCH 2/2] fix(avoidance): fix lateral distance calculation (#4820) * fix(avoidance): fix calculation logic for road shoulder distance Signed-off-by: satoshi-ota * fix(avoidance): fix overhang distance calculation Signed-off-by: satoshi-ota * fix(avoidance): don't set avoidable for invalid shift Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_planner/utils/avoidance/utils.hpp | 2 +- .../src/scene_module/avoidance/avoidance_module.cpp | 5 ++++- .../scene_module/lane_change/avoidance_by_lane_change.cpp | 2 +- .../behavior_path_planner/src/utils/avoidance/utils.cpp | 7 ++++--- 4 files changed, 10 insertions(+), 6 deletions(-) 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 e2934a02b63d8..0dc07f0950716 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 @@ -70,7 +70,7 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj); double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose); + const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose); void setEndData( AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx, 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 543f02abe730f..ad11dad090ff5 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 @@ -331,7 +331,7 @@ ObjectData AvoidanceModule::createObjectData( // 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); + object_data, data.reference_path, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; @@ -982,6 +982,9 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { avoid_lines.push_back(al_avoid); avoid_lines.push_back(al_return); + } else { + o.reason = "InvalidShiftLine"; + continue; } o.is_avoidable = true; 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 index 841fc9092e279..a8ff0c0bbcf02 100644 --- 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 @@ -216,7 +216,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( // 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); + object_data, data.reference_path, object_data.overhang_pose.position); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 856c3220165e7..945344259fdb2 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -406,13 +406,14 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( } double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose) + const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose) { double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number for (const auto & p : object_data.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); - const auto lateral = tier4_autoware_utils::calcLateralDeviation(base_pose, point); + const auto idx = findFirstNearestIndex(path.points, point); + const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() { if (lateral > largest_overhang) { @@ -1014,7 +1015,7 @@ void filterTargetObjects( const auto lines = rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); const auto & line = isOnRight(o) ? lines.back() : lines.front(); - const auto d = distance2d(to2D(overhang_basic_pose), to2D(line.basicLineString())); + const auto d = boost::geometry::distance(o.envelope_poly, to2D(line.basicLineString())); if (d < o.to_road_shoulder_distance) { o.to_road_shoulder_distance = d; target_line = line;