Skip to content

Commit

Permalink
refactor(ABLC): use common function
Browse files Browse the repository at this point in the history
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
satoshi-ota committed Oct 3, 2023
1 parent df3fb7f commit 6d09dca
Showing 1 changed file with 7 additions and 1 deletion.
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include "behavior_path_planner/utils/avoidance/utils.hpp"
#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp"
#include "behavior_path_planner/utils/path_utils.hpp"
#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp"

#include <lanelet2_extension/utility/utilities.hpp>
#include <tier4_autoware_utils/geometry/boost_geometry.hpp>
Expand Down Expand Up @@ -140,9 +141,14 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
{
const auto p = std::dynamic_pointer_cast<AvoidanceParameters>(avoidance_parameters_);

const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) {
const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
const auto lanelet_polygon = utils::toPolygon2d(lanelet);
return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
};
const auto [object_within_target_lane, object_outside_target_lane] =
utils::path_safety_checker::separateObjectsByLanelets(
*planner_data_->dynamic_object, data.current_lanelets);
*planner_data_->dynamic_object, data.current_lanelets, condition);

// 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
Expand Down

0 comments on commit 6d09dca

Please sign in to comment.