Skip to content

Commit

Permalink
remove unused functions
Browse files Browse the repository at this point in the history 
Signed-off-by: mohammad alqudah <[email protected]>
  • Loading branch information
@mkquda
mkquda committed Nov 25, 2024
1 parent 8f7bcbb commit 2dc863a
Show file tree
Hide file tree
Showing 2 changed files with 0 additions and 135 deletions.
9 changes: 0 additions & 9 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 @@ -156,15 +156,6 @@ bool is_delay_lane_change(
const std::vector<ExtendedPredictedObject> & target_objects,
CollisionCheckDebugMap & object_debug);

bool passed_parked_objects(
const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & object_debug);

std::optional<size_t> getLeadingStaticObjectIdx(
const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
const std::vector<ExtendedPredictedObject> & objects,
const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold);

lanelet::BasicPolygon2d create_polygon(
const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist);

Expand Down
126 changes: 0 additions & 126 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 @@ -800,132 +800,6 @@ bool is_delay_lane_change(
return false;
}

bool passed_parked_objects(
const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
const std::vector<ExtendedPredictedObject> & objects, CollisionCheckDebugMap & object_debug)
{
const auto route_handler = *common_data_ptr->route_handler_ptr;
const auto lane_change_parameters = *common_data_ptr->lc_param_ptr;
const auto & object_check_min_road_shoulder_width =
lane_change_parameters.delay.min_road_shoulder_width;
const auto & object_shiftable_ratio_threshold =
lane_change_parameters.delay.th_parked_vehicle_shift_ratio;
const auto & current_lane_path = common_data_ptr->current_lanes_path;

if (objects.empty() || lane_change_path.path.points.empty() || current_lane_path.points.empty()) {
return true;
}

const auto leading_obj_idx = getLeadingStaticObjectIdx(
route_handler, lane_change_path, objects, object_check_min_road_shoulder_width,
object_shiftable_ratio_threshold);
if (!leading_obj_idx) {
return true;
}

const auto & leading_obj = objects.at(*leading_obj_idx);
auto debug = utils::path_safety_checker::createObjectDebug(leading_obj);
const auto leading_obj_poly =
autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape);
if (leading_obj_poly.outer().empty()) {
return true;
}

const auto & current_path_end = current_lane_path.points.back().point.pose.position;
const auto dist_to_path_end = [&](const auto & src_point) {
if (common_data_ptr->lanes_ptr->current_lane_in_goal_section) {
const auto goal_pose = route_handler.getGoalPose();
return motion_utils::calcSignedArcLength(
current_lane_path.points, src_point, goal_pose.position);
}
return motion_utils::calcSignedArcLength(current_lane_path.points, src_point, current_path_end);
};

const auto min_dist_to_end_of_current_lane = std::invoke([&]() {
auto min_dist_to_end_of_current_lane = std::numeric_limits<double>::max();
for (const auto & point : leading_obj_poly.outer()) {
const auto obj_p = universe_utils::createPoint(point.x(), point.y(), 0.0);
const auto dist = dist_to_path_end(obj_p);
min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane);
}
return min_dist_to_end_of_current_lane;
});

// If there are still enough length after the target object, we delay the lane change
if (min_dist_to_end_of_current_lane <= common_data_ptr->transient_data.current_dist_buffer.min) {
return true;
}

const auto current_pose = common_data_ptr->get_ego_pose();
const auto dist_ego_to_obj = motion_utils::calcSignedArcLength(
current_lane_path.points, current_pose.position, leading_obj.initial_pose.position);

if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) {
return true;
}

debug.second.unsafe_reason = "delay lane change";
utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false);
return false;
}

std::optional<size_t> getLeadingStaticObjectIdx(
const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
const std::vector<ExtendedPredictedObject> & objects,
const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold)
{
const auto & path = lane_change_path.path;

if (path.points.empty() || objects.empty()) {
return {};
}

const auto & lane_change_start = lane_change_path.info.lane_changing_start;
const auto & path_end = path.points.back();

double dist_lc_start_to_leading_obj = 0.0;
std::optional<size_t> leading_obj_idx = std::nullopt;
for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) {
const auto & obj = objects.at(obj_idx);
const auto & obj_pose = obj.initial_pose;

// ignore non-static object
// TODO(shimizu): parametrize threshold
const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y);
if (obj_vel_norm > 1.0) {
continue;
}

// ignore non-parked object
if (!isParkedObject(
path, route_handler, obj, object_check_min_road_shoulder_width,
object_shiftable_ratio_threshold)) {
continue;
}

const double dist_back_to_obj = autoware::motion_utils::calcSignedArcLength(
path.points, path_end.point.pose.position, obj_pose.position);
if (dist_back_to_obj > 0.0) {
// object is not on the lane change path
continue;
}

const double dist_lc_start_to_obj = autoware::motion_utils::calcSignedArcLength(
path.points, lane_change_start.position, obj_pose.position);
if (dist_lc_start_to_obj < 0.0) {
// object is on the lane changing path or behind it. It will be detected in safety check
continue;
}

if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) {
dist_lc_start_to_leading_obj = dist_lc_start_to_obj;
leading_obj_idx = obj_idx;
}
}

return leading_obj_idx;
}

lanelet::BasicPolygon2d create_polygon(
const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist)
{
Expand Down

0 comments on commit 2dc863a

Please sign in to comment.