diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index f30bf31d4e010..1a7350d82aa4e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -209,6 +209,10 @@ class LaneChangeBase return direction_; } + boost::optional getStopPose() const { return lane_change_stop_pose_; } + + void resetStopPose() { lane_change_stop_pose_ = boost::none; } + protected: virtual lanelet::ConstLanelets getCurrentLanes() const = 0; @@ -244,6 +248,7 @@ class LaneChangeBase PathWithLaneId prev_module_path_{}; DrivableAreaInfo prev_drivable_area_info_{}; TurnSignalInfo prev_turn_signal_info_{}; + boost::optional lane_change_stop_pose_{boost::none}; PathWithLaneId prev_approved_path_{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 664f547edb92d..747c75babb573 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -153,6 +153,8 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; + void setStopPose(const Pose & stop_pose); + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 0ceab21573316..5e47116117103 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -203,6 +203,7 @@ void LaneChangeInterface::updateData() module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateSpecialData(); + module_type_->resetStopPose(); } BehaviorModuleOutput LaneChangeInterface::plan() @@ -221,6 +222,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() *prev_approved_path_ = *getPreviousModuleOutput().path; module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path); + stop_pose_ = module_type_->getStopPose(); + updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -249,6 +252,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() path_reference_ = getPreviousModuleOutput().reference_path; + stop_pose_ = module_type_->getStopPose(); + if (!module_type_->isValidPath()) { removeRTCStatus(); path_candidate_ = std::make_shared(); @@ -286,6 +291,7 @@ void LaneChangeInterface::updateModuleParams(const std::any & parameters) void LaneChangeInterface::setData(const std::shared_ptr & data) { + planner_data_ = data; module_type_->setData(data); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index c82532c6be8ef..dea6073654ae2 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -138,6 +138,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); + setStopPose(stop_point.point.pose); } const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); @@ -198,6 +199,7 @@ void NormalLaneChange::insertStopPoint( const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; if (stopping_distance > 0.0) { const auto stop_point = utils::insertStopPoint(stopping_distance, path); + setStopPose(stop_point.point.pose); } } @@ -649,7 +651,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const lanelet::ConstLanelets & target_backward_lanes) const { const auto current_pose = getEgoPose(); - const auto current_vel = getEgoVelocity(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; const auto & objects = *planner_data_->dynamic_object; @@ -672,12 +673,6 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto target_backward_polygon = utils::lane_change::createPolygon( target_backward_lanes, 0.0, std::numeric_limits::max()); - // minimum distance to lane changing start point - const double t_prepare = common_parameters.lane_change_prepare_duration; - const double a_min = lane_change_parameters_->min_longitudinal_acc; - const double min_dist_to_lane_changing_start = std::max( - current_vel * t_prepare, current_vel * t_prepare + 0.5 * a_min * t_prepare * t_prepare); - auto filtered_objects = objects; utils::path_safety_checker::filterObjectsByClass( @@ -712,12 +707,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // check if the object intersects with target lanes if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { - // ignore static parked object that are in front of the ego vehicle in target lanes - bool is_parked_object = - utils::lane_change::isParkedObject(target_path, route_handler, extended_object, 0.0, 0.0); - if (is_parked_object && min_dist_ego_to_obj > min_dist_to_lane_changing_start) { - continue; - } + // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target + // lanes filtered_obj_indices.target_lane.push_back(i); continue; @@ -1466,4 +1457,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return path_safety_status; } + +void NormalLaneChange::setStopPose(const Pose & stop_pose) +{ + lane_change_stop_pose_ = stop_pose; +} + } // namespace behavior_path_planner