diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 18a797976161c..02280f7ffa697 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -195,7 +195,7 @@ if (max_lane_change_length > ego's distance to the end of the current lanes.) t stop endif -if (isVehicleStuck(current_lanes)) then (yes) +if ego is stuck in the current lanes then (yes) :Return **sampled acceleration values**; stop else (no) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 3ec74461e014b..0b4d6deea7a64 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -89,10 +89,6 @@ class NormalLaneChange : public LaneChangeBase bool isRequiredStop(const bool is_trailing_object) override; - bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const override; - bool hasFinishedLaneChange() const override; bool isAbleToReturnCurrentLane() const override; @@ -127,9 +123,7 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - std::vector calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + std::vector calc_prepare_durations() const; lane_change::TargetObjects getTargetObjects( const FilteredByLanesExtendedObjects & predicted_objects, @@ -150,10 +144,6 @@ class NormalLaneChange : public LaneChangeBase const double target_lane_length, const double lane_changing_length, const double lane_changing_velocity, const double buffer_for_next_lane_change) const; - bool hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; - std::vector get_prepare_metrics() const; std::vector get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, @@ -167,8 +157,7 @@ class NormalLaneChange : public LaneChangeBase const Pose & lc_start_pose, const double shift_length) const; bool check_candidate_path_safety( - const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const bool is_stuck) const; + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; std::optional calcTerminalLaneChangePath( const lanelet::ConstLanelets & current_lanes, @@ -193,15 +182,9 @@ class NormalLaneChange : public LaneChangeBase const RSSparams & selected_rss_param, const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const; - //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. - //! @param obstacle_check_distance Distance to check ahead for any objects that might be - //! obstructing ego path. It makes sense to use values like the maximum lane change distance. - bool isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; - double get_max_velocity_for_safety_check() const; - bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + bool is_ego_stuck() const; /** * @brief Checks if the given pose is a valid starting point for a lane change. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 580c5709cb5c1..4816a2f6c4eac 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -112,10 +112,6 @@ class LaneChangeBase virtual PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( PathSafetyStatus approve_path_safety_status) = 0; - virtual bool isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const = 0; - virtual bool isStoppedAtRedTrafficLight() const = 0; virtual bool calcAbortPath() = 0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 5c8b70c59cdc7..6b4b7c4f2dc45 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -344,6 +344,7 @@ struct TransientData lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes bool is_ego_near_current_terminal_start{false}; + bool is_ego_stuck{false}; }; using RouteHandlerPtr = std::shared_ptr; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index af816bf621582..23ae850189e2a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -76,8 +76,8 @@ void LaneChangeInterface::updateData() universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); - module_type_->update_transient_data(); module_type_->update_filtered_objects(); + module_type_->update_transient_data(); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index db375651c4e3a..d49d05f2a6f18 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -141,6 +141,9 @@ void NormalLaneChange::update_transient_data() transient_data.is_ego_near_current_terminal_start = transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + updateStopTime(); + transient_data.is_ego_stuck = is_ego_stuck(); + RCLCPP_DEBUG( logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); RCLCPP_DEBUG( @@ -168,7 +171,6 @@ void NormalLaneChange::update_filtered_objects() void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -731,35 +733,6 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( lane_change_lane.value(), getEgoPose(), backward_length, forward_length); } -bool NormalLaneChange::isNearEndOfCurrentLanes( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const double threshold) const -{ - if (current_lanes.empty()) { - return false; - } - - const auto & route_handler = getRouteHandler(); - const auto & current_pose = getEgoPose(); - - // TODO(Azu) fully change to transient data - const auto distance_to_lane_change_end = std::invoke([&]() { - auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); - - if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { - distance_to_end = std::min( - distance_to_end, - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); - } - - return std::max(0.0, distance_to_end) - - common_data_ptr_->transient_data.current_dist_buffer.min; - }); - - lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; - return distance_to_lane_change_end < threshold; -} - bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); @@ -963,6 +936,7 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons std::vector NormalLaneChange::sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { + // TODO(Azu): sampler should work even when we're not approaching terminal if (prev_module_output_.path.points.empty()) { return {}; } @@ -986,7 +960,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( const auto current_max_dist_buffer = calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - if (current_max_dist_buffer > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (current_max_dist_buffer > common_data_ptr_->transient_data.dist_to_terminal_end) { RCLCPP_DEBUG( logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); @@ -995,7 +969,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (isVehicleStuck(current_lanes)) { + if (common_data_ptr_->transient_data.is_ego_stuck) { auto clock = rclcpp::Clock(RCL_ROS_TIME); RCLCPP_INFO_THROTTLE( logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, @@ -1023,22 +997,23 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } -std::vector NormalLaneChange::calcPrepareDuration( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +std::vector NormalLaneChange::calc_prepare_durations() const { - const auto base_link2front = planner_data_->parameters.base_link2front; - const auto threshold = - lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + const auto threshold = common_data_ptr_->bpp_param_ptr->base_link2front + + lc_param_ptr->min_length_for_turn_signal_activation; + const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; + + // TODO(Azu) this check seems to cause scenario failures. + if (common_data_ptr_->transient_data.dist_to_terminal_start >= threshold) { + return {max_prepare_duration}; + } std::vector prepare_durations; constexpr double step = 0.5; - for (double duration = lane_change_parameters_->lane_change_prepare_duration; duration >= 0.0; - duration -= step) { + for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) { prepare_durations.push_back(duration); - if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { - break; - } } return prepare_durations; @@ -1083,12 +1058,11 @@ bool NormalLaneChange::get_prepare_segment( lane_change::TargetObjects NormalLaneChange::getTargetObjects( const FilteredByLanesExtendedObjects & filtered_objects, - const lanelet::ConstLanelets & current_lanes) const + [[maybe_unused]] const lanelet::ConstLanelets & current_lanes) const { ExtendedPredictedObjects leading_objects = filtered_objects.target_lane_leading; - const auto is_stuck = isVehicleStuck(current_lanes); const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; - if (chk_obj_in_curr_lanes || is_stuck) { + if (chk_obj_in_curr_lanes || common_data_ptr_->transient_data.is_ego_stuck) { leading_objects.insert( leading_objects.end(), filtered_objects.current_lane.begin(), filtered_objects.current_lane.end()); @@ -1133,8 +1107,7 @@ FilteredByLanesExtendedObjects NormalLaneChange::filterObjects() const return {}; } - const auto path = - route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto & path = common_data_ptr_->current_lanes_path; auto filtered_by_lanes_objects = filterObjectsByLanelets(objects, path); @@ -1359,43 +1332,6 @@ PathWithLaneId NormalLaneChange::getTargetSegment( return target_segment; } -bool NormalLaneChange::hasEnoughLength( - const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, [[maybe_unused]] const Direction direction) const -{ - if (target_lanes.empty()) { - return false; - } - - const auto current_pose = getEgoPose(); - const auto & route_handler = getRouteHandler(); - const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); - const auto minimum_lane_change_length_to_preferred_lane = - common_data_ptr_->transient_data.next_dist_buffer.min; - - const double lane_change_length = path.info.length.sum(); - if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { - return false; - } - - const auto goal_pose = route_handler->getGoalPose(); - if ( - route_handler->isInGoalRouteSection(current_lanes.back()) && - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getSignedDistance(current_pose, goal_pose, current_lanes)) { - return false; - } - - // return if there are no target lanes - if ( - lane_change_length + minimum_lane_change_length_to_preferred_lane > - utils::getDistanceToEndOfLane(current_pose, target_lanes)) { - return false; - } - - return true; -} - std::vector NormalLaneChange::get_prepare_metrics() const { const auto & current_lanes = common_data_ptr_->lanes_ptr->current; @@ -1406,7 +1342,7 @@ std::vector NormalLaneChange::get_prepare_metrics() cons const auto longitudinal_acc_sampling_values = sampleLongitudinalAccValues(current_lanes, target_lanes); - const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); + const auto prepare_durations = calc_prepare_durations(); RCLCPP_DEBUG( logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", @@ -1467,7 +1403,6 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto & current_lanes = get_current_lanes(); const auto & target_lanes = get_target_lanes(); - const auto is_stuck = isVehicleStuck(current_lanes); const auto current_velocity = getEgoVelocity(); const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = getTargetObjects(filtered_objects_, current_lanes); @@ -1554,7 +1489,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) candidate_paths.push_back(candidate_path); try { - if (check_candidate_path_safety(candidate_path, target_objects, is_stuck)) { + if (check_candidate_path_safety(candidate_path, target_objects)) { debug_print_lat("ACCEPT!!!: it is valid and safe!"); return true; } @@ -1577,7 +1512,6 @@ LaneChangePath NormalLaneChange::get_candidate_path( const Pose & lc_start_pose, const double shift_length) const { const auto & route_handler = *getRouteHandler(); - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto resample_interval = @@ -1608,7 +1542,10 @@ LaneChangePath NormalLaneChange::get_candidate_path( throw std::logic_error("failed to generate candidate path!"); } - if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction_)) { + if ( + candidate_path.value().info.length.sum() + + common_data_ptr_->transient_data.next_dist_buffer.min > + common_data_ptr_->transient_data.dist_to_terminal_end) { throw std::logic_error("invalid candidate path length!"); } @@ -1616,9 +1553,9 @@ LaneChangePath NormalLaneChange::get_candidate_path( } bool NormalLaneChange::check_candidate_path_safety( - const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects, - const bool is_stuck) const + const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { + const auto is_stuck = common_data_ptr_->transient_data.is_ego_stuck; if ( !is_stuck && !utils::lane_change::passed_parked_objects( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, @@ -1874,10 +1811,9 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( - isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && - isAbleToStopSafely() && is_trailing_object) { + common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() && + is_trailing_object) { current_lane_change_state_ = LaneChangeStates::Stop; return true; } @@ -2190,63 +2126,6 @@ bool NormalLaneChange::is_collided( return !is_collided; } -// Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes -bool NormalLaneChange::isVehicleStuck( - const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const -{ - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - // Ego is still moving, not in stuck - if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { - RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); - return false; - } - - // Ego is just stopped, not sure it is in stuck yet. - if (getStopTime() < lane_change_parameters_->stop_time_threshold) { - RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); - return false; - } - - // Check if any stationary object exist in obstacle_check_distance - const auto base_distance = common_data_ptr_->transient_data.current_lanes_ego_arc.length; - - for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { - const auto & p = object.initial_pose; // TODO(Horibe): consider footprint point - - // Note: it needs chattering prevention. - if (std::abs(object.initial_twist.linear.x) > 0.3) { // check if stationary - continue; - } - - const auto ego_to_obj_dist = - lanelet::utils::getArcCoordinates(current_lanes, p).length - base_distance; - if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { - RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); - return true; // Stationary object is in front of ego. - } - } - - // Check if Ego is in terminal of current lanes - const auto & route_handler = getRouteHandler(); - const double distance_to_terminal = - route_handler->isInGoalRouteSection(current_lanes.back()) - ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) - : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const double terminal_judge_buffer = current_min_dist_buffer + stop_point_buffer + 1.0; - if (distance_to_terminal < terminal_judge_buffer) { - return true; - } - - // No stationary objects found in obstacle_check_distance and Ego is not in terminal of current - RCLCPP_DEBUG( - logger_, - "No stationary objects found in obstacle_check_distance and Ego is not in " - "terminal of current lanes"); - return false; -} - double NormalLaneChange::get_max_velocity_for_safety_check() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -2259,35 +2138,56 @@ double NormalLaneChange::get_max_velocity_for_safety_check() const return getCommonParam().max_vel; } -bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::is_ego_stuck() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (current_lanes.empty()) { - lane_change_debug_.is_stuck = false; - return false; // can not check + const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; + + if (std::abs(common_data_ptr_->get_ego_speed()) > lc_param_ptr->stop_velocity_threshold) { + RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); + return false; } - const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - const auto current_max_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.max; - const auto rss_dist = calcRssDistance( - 0.0, lane_change_parameters_->minimum_lane_changing_velocity, - lane_change_parameters_->rss_params); + // Ego is just stopped, not sure it is in stuck yet. + if (getStopTime() < lc_param_ptr->stop_time_threshold) { + RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); + return false; + } + + // Check if any stationary object exist in obstacle_check_distance + const auto & current_lanes_path = common_data_ptr_->current_lanes_path; + const auto & ego_pose = common_data_ptr_->get_ego_pose(); + const auto rss_dist = + calcRssDistance(0.0, lc_param_ptr->minimum_lane_changing_velocity, lc_param_ptr->rss_params); // It is difficult to define the detection range. If it is too short, the stuck will not be // determined, even though you are stuck by an obstacle. If it is too long, // the ego will be judged to be stuck by a distant vehicle, even though the ego is only // stopped at a traffic light. Essentially, the calculation should be based on the information of // the stop reason, but this is outside the scope of one module. I keep it as a TODO. - constexpr double DETECTION_DISTANCE_MARGIN = 10.0; - const auto detection_distance = current_max_dist_buffer + rss_dist + - getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; - RCLCPP_DEBUG( - logger_, "current_max_dist_buffer: %f, max_acc: %f", current_max_dist_buffer, max_acc); + constexpr auto detection_distance_margin = 10.0; + const auto obstacle_check_distance = common_data_ptr_->transient_data.lane_changing_length.max + + rss_dist + common_data_ptr_->bpp_param_ptr->base_link2front + + detection_distance_margin; + const auto has_object_blocking = std::any_of( + filtered_objects_.current_lane.begin(), filtered_objects_.current_lane.end(), + [&](const auto & object) { + // Note: it needs chattering prevention. + if ( + std::abs(object.initial_twist.linear.x) > + lc_param_ptr->stopped_object_velocity_threshold) { // check if stationary + return false; + } - auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); + const auto ego_to_obj_dist = + calcSignedArcLength( + current_lanes_path.points, ego_pose.position, object.initial_pose.position) - + obstacle_check_distance; + return ego_to_obj_dist < 0.0; + }); - lane_change_debug_.is_stuck = is_vehicle_stuck; - return is_vehicle_stuck; + lane_change_debug_.is_stuck = has_object_blocking; + return has_object_blocking; } bool NormalLaneChange::is_valid_start_point(