Skip to content

Commit

Permalink
filter the path directly
Browse files Browse the repository at this point in the history 
Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed Dec 27, 2024
1 parent 881afe1 commit 6453e15
Show file tree
Hide file tree
Showing 4 changed files with 76 additions and 69 deletions.
4 changes: 0 additions & 4 deletions ...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,10 +87,6 @@ bool path_footprint_exceeds_target_lane_bound(
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info,
const double margin = 0.1);

void filter_out_of_bound_trajectories(
const CommonDataPtr & common_data_ptr,
std::vector<lane_change::TrajectoryGroup> & trajectory_groups);

std::vector<DrivableLanes> generateDrivableLanes(
const std::vector<DrivableLanes> & original_drivable_lanes, const RouteHandler & route_handler,
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes);
Expand Down
5 changes: 4 additions & 1 deletion ...ing/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <autoware/behavior_path_lane_change_module/utils/markers.hpp>
#include <autoware/universe_utils/ros/marker_helper.hpp>
#include <autoware_lanelet2_extension/visualization/visualization.hpp>
#include <magic_enum.hpp>

#include <autoware_perception_msgs/msg/predicted_objects.hpp>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
Expand Down Expand Up @@ -89,8 +90,10 @@ MarkerArray showAllValidLaneChangePath(
text_marker.id = ++id;
text_marker.pose = points.at(lc_idx).point.pose;
text_marker.text = fmt::format(
"vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | lat_acc: {lat_acc:.3f}[m/s2] | t: "
"type: {type} | vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | lat_acc: "
"{lat_acc:.3f}[m/s2] | t: "
"{time:.3f}[s] | L: {length:.3f}[m]",
fmt::arg("type", magic_enum::enum_name(lc_path.type)),
fmt::arg("vel", info.velocity.lane_changing),
fmt::arg("lon_acc", info.longitudinal_acceleration.lane_changing),
fmt::arg("lat_acc", info.lateral_acceleration), fmt::arg("time", info.duration.lane_changing),
Expand Down
73 changes: 72 additions & 1 deletion planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <autoware/universe_utils/system/stop_watch.hpp>
#include <autoware_frenet_planner/frenet_planner.hpp>
#include <range/v3/action/insert.hpp>
#include <range/v3/action/remove_if.hpp>
#include <range/v3/algorithm.hpp>
#include <range/v3/numeric/accumulate.hpp>
#include <range/v3/view.hpp>
Expand All @@ -46,10 +47,14 @@ using autoware::behavior_path_planner::lane_change::LCParamPtr;

using autoware::behavior_path_planner::LaneChangePhaseMetrics;
using autoware::behavior_path_planner::ShiftLine;
using autoware::behavior_path_planner::lane_change::TrajectoryGroup;
using autoware::frenet_planner::FrenetState;
using autoware::frenet_planner::SamplingParameters;
using autoware::route_handler::Direction;
using autoware::sampler_common::FrenetPoint;
using autoware::sampler_common::transform::Spline2D;
using autoware::universe_utils::LineString2d;
using autoware::universe_utils::Point2d;
using geometry_msgs::msg::Pose;

double calc_resample_interval(
Expand Down Expand Up @@ -227,6 +232,62 @@ double calc_average_curvature(const std::vector<double> & curvatures)
const auto count_k = static_cast<double>(ranges::distance(filtered_k));
return sum_of_k / count_k;
}

LineString2d get_linestring_bound(const lanelet::ConstLanelets & lanes, const Direction direction)
{
LineString2d line_string;
const auto & get_bound = [&](const lanelet::ConstLanelet & lane) {
return (direction == Direction::LEFT) ? lane.leftBound2d() : lane.rightBound2d();
};

const auto reserve_size = ranges::accumulate(
lanes, 0UL,
[&](auto sum, const lanelet::ConstLanelet & lane) { return sum + get_bound(lane).size(); });
line_string.reserve(reserve_size);
for (const auto & lane : lanes) {
ranges::for_each(get_bound(lane), [&line_string](const auto & point) {
boost::geometry::append(line_string, Point2d(point.x(), point.y()));
});
}
return line_string;
}

Point2d shift_point(const Point2d & p1, const Point2d & p2, const double offset)
{
// Calculate the perpendicular vector
double dx = p2.x() - p1.x();
double dy = p2.y() - p1.y();
double length = std::sqrt(dx * dx + dy * dy);

// Normalize and find the perpendicular direction
double nx = -dy / length;
double ny = dx / length;

return {p1.x() + nx * offset, p1.y() + ny * offset};
}

bool check_out_of_bound_paths(
const CommonDataPtr & common_data_ptr, const std::vector<Pose> & lane_changing_poses,
const LineString2d & lane_boundary, const bool shift_to_left)
{
const auto distance = (0.5 * common_data_ptr->bpp_param_ptr->vehicle_width + 0.1);
const auto offset = (shift_to_left ? 1.0 : -1.0) * distance; // invert direction
if (lane_changing_poses.size() <= 2) {
return true; // Remove candidates with insufficient poses
}

LineString2d path_ls;
path_ls.reserve(lane_changing_poses.size());

const auto segments = lane_changing_poses | ranges::views::sliding(2);
ranges::for_each(segments | ranges::views::drop(1), [&](const auto & segment) {
const auto & p1 = segment[0].position;
const auto & p2 = segment[1].position;
boost::geometry::append(path_ls, shift_point({p2.x, p2.y}, {p1.x, p1.y}, offset));
});

return boost::geometry::disjoint(path_ls, lane_boundary); // Remove if disjoint
}
}; // namespace

namespace autoware::behavior_path_planner::utils::lane_change
Expand Down Expand Up @@ -410,7 +471,11 @@ std::vector<lane_change::TrajectoryGroup> generate_frenet_candidates(
universe_utils::StopWatch<std::chrono::microseconds> sw;

const auto & transient_data = common_data_ptr->transient_data;
const auto & current_lanes = common_data_ptr->lanes_ptr->current;
const auto & target_lanes = common_data_ptr->lanes_ptr->target;
const auto direction = common_data_ptr->direction;
const auto current_lane_boundary = get_linestring_bound(current_lanes, direction);
const auto is_shift_to_left = direction == Direction::LEFT;

trajectory_groups.reserve(metrics.size());
for (const auto & metric : metrics) {
Expand Down Expand Up @@ -479,6 +544,13 @@ std::vector<lane_change::TrajectoryGroup> generate_frenet_candidates(
}
}

const auto out_of_bound = check_out_of_bound_paths(
common_data_ptr, traj.poses, current_lane_boundary, is_shift_to_left);

if (out_of_bound) {
continue;
}

trajectory_groups.emplace_back(
prepare_segment, target_lane_reference_path, target_ref_path_sums, metric, traj,
initial_state);
Expand All @@ -500,7 +572,6 @@ std::vector<lane_change::TrajectoryGroup> generate_frenet_candidates(
calc_average_curvature(p2.lane_changing.curvatures);
});

utils::lane_change::filter_out_of_bound_trajectories(common_data_ptr, trajectory_groups);
return trajectory_groups;
}

Check warning on line 576 in planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method 

generate_frenet_candidates has a cyclomatic complexity of 17, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 576 in planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead 

generate_frenet_candidates has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check warning on line 576 in planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Deep, Nested Complexity 

generate_frenet_candidates has a nested complexity depth of 4, threshold = 4. This function contains deeply nested logic such as if statements and/or loops. The deeper the nesting, the lower the code health.

Expand Down
63 changes: 0 additions & 63 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,69 +162,6 @@ bool path_footprint_exceeds_target_lane_bound(
return false;
}

void filter_out_of_bound_trajectories(
const CommonDataPtr & common_data_ptr,
std::vector<lane_change::TrajectoryGroup> & trajectory_groups)
{
const auto lane_boundary = std::invoke([&]() {
universe_utils::LineString2d line_string;
const auto & get_bound = [&](const lanelet::ConstLanelet & lane) {
const auto direction = common_data_ptr->direction;
return (direction == Direction::LEFT) ? lane.leftBound2d() : lane.rightBound2d();
};

const auto & lanes = common_data_ptr->lanes_ptr->current;

const auto reserve_size = ranges::accumulate(
lanes, 0UL,
[&](auto sum, const lanelet::ConstLanelet & lane) { return sum + get_bound(lane).size(); });
line_string.reserve(reserve_size);
for (const auto & lane : lanes) {
const auto & bound =
(common_data_ptr->direction == Direction::LEFT) ? lane.leftBound2d() : lane.rightBound2d();
ranges::for_each(get_bound(lane), [&line_string](const auto & point) {
boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y()));
});
}
return line_string;
});

const auto shift_point = [&](const Point2d & p1, const Point2d & p2) {
const auto direction = common_data_ptr->direction;
const auto left_side = direction == Direction::LEFT;
const auto distance = (0.5 * common_data_ptr->bpp_param_ptr->vehicle_width + 0.1);
const auto offset = (left_side ? 1.0 : -1.0) * distance; // invert direction
// Calculate the perpendicular vector
double dx = p2.x() - p1.x();
double dy = p2.y() - p1.y();
double length = std::sqrt(dx * dx + dy * dy);

// Normalize and find the perpendicular direction
double nx = -dy / length;
double ny = dx / length;

return Point2d(p1.x() + nx * offset, p1.y() + ny * offset);
};

trajectory_groups |= ranges::actions::remove_if([&](const TrajectoryGroup & candidate) {
if (candidate.lane_changing.poses.size() <= 2) {
return true; // Remove candidates with insufficient poses
}

universe_utils::LineString2d path_ls;
path_ls.reserve(candidate.lane_changing.poses.size());

const auto segments = candidate.lane_changing.poses | ranges::views::sliding(2);
ranges::for_each(segments | ranges::views::drop(1), [&](const auto & segment) {
const auto & p1 = segment[0].position;
const auto & p2 = segment[1].position;
boost::geometry::append(path_ls, shift_point({p2.x, p2.y}, {p1.x, p1.y}));
});

return boost::geometry::disjoint(path_ls, lane_boundary); // Remove if disjoint
});
}

std::vector<DrivableLanes> generateDrivableLanes(
const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & lane_change_lanes)
Expand Down

0 comments on commit 6453e15

Please sign in to comment.