Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(lane_change): add visualization for passParkedObject #939

Merged
merged 2 commits into from
Oct 14, 2023
Merged

fix(lane_change): add visualization for passParkedObject #939

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2933199
fix(lane_change): fix force path not appear (#5288)
zulfaqar-azmi-t4 Oct 13, 2023
7f6c6c6
fix(lane_change): add visualization for passParkedObject (#5291)
zulfaqar-azmi-t4 Oct 13, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ using data::lane_change::PathSafetyStatus;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::Twist;
using path_safety_checker::CollisionCheckDebugMap;
using route_handler::Direction;
using tier4_autoware_utils::Polygon2d;

Expand Down Expand Up @@ -175,7 +176,8 @@ bool isParkedObject(
bool passParkedObject(
const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
const std::vector<ExtendedPredictedObject> & objects, const double minimum_lane_change_length,
const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters);
const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters,
CollisionCheckDebugMap & object_debug);

boost::optional<size_t> getLeadingStaticObjectIdx(
const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
Expand Down
4 changes: 2 additions & 2 deletions planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1130,12 +1130,12 @@ bool NormalLaneChange::getLaneChangePaths(
logger_, "Stop time is over threshold. Allow lane change in intersection.");
}

candidate_paths->push_back(*candidate_path);
if (utils::lane_change::passParkedObject(
route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer,
is_goal_in_route, *lane_change_parameters_)) {
is_goal_in_route, *lane_change_parameters_, object_debug_)) {
return false;
}
candidate_paths->push_back(*candidate_path);

if (!check_safety) {
return false;
Expand Down
6 changes: 5 additions & 1 deletion planning/behavior_path_planner/src/utils/lane_change/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -946,7 +946,8 @@ bool isParkedObject(
bool passParkedObject(
const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
const std::vector<ExtendedPredictedObject> & objects, const double minimum_lane_change_length,
const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters)
const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters,
CollisionCheckDebugMap & object_debug)
{
const auto & object_check_min_road_shoulder_width =
lane_change_parameters.object_check_min_road_shoulder_width;
Expand All @@ -969,6 +970,7 @@ bool passParkedObject(
}

const auto & leading_obj = objects.at(*leading_obj_idx);
auto debug = marker_utils::createObjectDebug(leading_obj);
const auto leading_obj_poly =
tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape);
if (leading_obj_poly.outer().empty()) {
Expand All @@ -992,6 +994,8 @@ bool passParkedObject(

// If there are still enough length after the target object, we delay the lane change
if (min_dist_to_end_of_current_lane > minimum_lane_change_length) {
debug.second.unsafe_reason = "delay lane change";
marker_utils::updateCollisionCheckDebugMap(object_debug, debug, false);
return true;
}

Expand Down