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

Jump to bottom

fix(behavior_path_planner): fix lane change path validation #2421

Merged
merged 4 commits into from
Dec 1, 2022
Merged

fix(behavior_path_planner): fix lane change path validation #2421

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b3046b1
fix(behavior_path_planner): clip_path_for_goal
rej55 Dec 1, 2022
8fb79ee
fix(behavior_path_planner): clip path for validation
rej55 Dec 1, 2022
1ecf5f5
fix(behavior_path_planner): use path point only to check lane departure
rej55 Dec 1, 2022
007135e
fix(behavior_path_planner): remove unnecessary change
rej55 Dec 1, 2022
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
3 changes: 0 additions & 3 deletions ...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
#include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
#include "behavior_path_planner/scene_module/scene_module_interface.hpp"
#include "behavior_path_planner/turn_signal_decider.hpp"
#include "lane_departure_checker/lane_departure_checker.hpp"

#include <rclcpp/rclcpp.hpp>

Expand All @@ -44,7 +43,6 @@ namespace behavior_path_planner
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::Twist;
using lane_departure_checker::LaneDepartureChecker;
using marker_utils::CollisionCheckDebug;
using tier4_planning_msgs::msg::LaneChangeDebugMsg;
using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
Expand Down Expand Up @@ -105,7 +103,6 @@ class LaneChangeModule : public SceneModuleInterface
LaneChangeStatus status_;
PathShifter path_shifter_;
mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_;
LaneDepartureChecker lane_departure_checker_;

double lane_change_lane_length_{200.0};
double check_distance_{100.0};
Expand Down
19 changes: 15 additions & 4 deletions planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,6 @@ LaneChangeModule::LaneChangeModule(
uuid_right_{generateUUID()}
{
steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(&node, "lane_change");
lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo());
}

BehaviorModuleOutput LaneChangeModule::run()
Expand Down Expand Up @@ -146,6 +145,7 @@ BehaviorModuleOutput LaneChangeModule::plan()
{
constexpr double resample_interval{1.0};
auto path = util::resamplePathWithSpline(status_.lane_change_path.path, resample_interval);

if (!isValidPath(path)) {
status_.is_safe = false;
return BehaviorModuleOutput{};
Expand Down Expand Up @@ -387,9 +387,20 @@ bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const
drivable_lanes, parameters_->drivable_area_left_bound_offset,
parameters_->drivable_area_right_bound_offset);
const auto lanelets = util::transformToLanelets(expanded_lanes);
if (lane_departure_checker_.checkPathWillLeaveLane(lanelets, path)) {
RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes");
return false;

// check path points are in any lanelets
for (const auto & point : path.points) {
bool is_in_lanelet = false;
for (const auto & lanelet : lanelets) {
if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) {
is_in_lanelet = true;
break;
}
}
if (!is_in_lanelet) {
RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes");
return false;
}
}

// check relative angle
Expand Down
14 changes: 12 additions & 2 deletions planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -496,7 +496,12 @@ PathWithLaneId getReferencePathFromTargetLane(
const ArcCoordinates lane_change_start_arc_position =
lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose);
const double & s_start = lane_change_start_arc_position.length;
const double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length;
double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length;
if (route_handler.isInGoalRouteSection(target_lanes.back())) {
const auto goal_arc_coordinates =
lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose());
s_end = std::min(s_end, goal_arc_coordinates.length);
}
return route_handler.getCenterLinePath(target_lanes, s_start, s_end);
}

Expand All @@ -511,7 +516,12 @@ PathWithLaneId getReferencePathFromTargetLane(
lanelet::utils::getArcCoordinates(target_lanes, in_target_end_pose);

const double & s_start = lane_change_start_arc_position.length;
const double & s_end = lane_change_end_arc_position.length;
double s_end = lane_change_end_arc_position.length;
if (route_handler.isInGoalRouteSection(target_lanes.back())) {
const auto goal_arc_coordinates =
lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose());
s_end = std::min(s_end, goal_arc_coordinates.length);
}
return route_handler.getCenterLinePath(target_lanes, s_start, s_end);
}

Expand Down