diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 4a347e85b07eb..41df033693baa 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -211,16 +211,17 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral #### parameters for shift pull out -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | -| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | false | -| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | -| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | -| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | -| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | +| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true | +| allow_check_shift_path_lane_departure_override | [-] | bool | flag to override/cancel lane departure check if the ego vehicle's starting pose is already out of lane | false | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | +| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 | ### **geometric pull out** diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index c14e06f140f60..bbbf182d1c87c 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -12,6 +12,8 @@ center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true + check_shift_path_lane_departure: true + allow_check_shift_path_lane_departure_override: false shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index 97217060d3c20..d096ce7f53009 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -178,6 +178,21 @@ class StartPlannerModule : public SceneModuleInterface bool requiresDynamicObjectsCollisionDetection() const; + uint16_t getSteeringFactorDirection( + const behavior_path_planner::BehaviorModuleOutput & output) const + { + switch (output.turn_signal_info.turn_signal.command) { + case TurnIndicatorsCommand::ENABLE_LEFT: + return SteeringFactor::LEFT; + + case TurnIndicatorsCommand::ENABLE_RIGHT: + return SteeringFactor::RIGHT; + + default: + return SteeringFactor::STRAIGHT; + } + }; + /** * @brief Check if there are no moving objects around within a certain radius. * diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index d3b95c3358bab..dd67c75c766e9 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -50,6 +50,7 @@ struct StartPlannerParameters // shift pull out bool enable_shift_pull_out{false}; bool check_shift_path_lane_departure{false}; + bool allow_check_shift_path_lane_departure_override{false}; double shift_collision_check_distance_from_end{0.0}; double minimum_shift_pull_out_distance{0.0}; int lateral_acceleration_sampling_num{0}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 7fb52d59c418b..3326a0b403f5d 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -53,6 +53,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); p.shift_collision_check_distance_from_end = node->declare_parameter(ns + "shift_collision_check_distance_from_end"); p.minimum_shift_pull_out_distance = @@ -335,14 +337,333 @@ void StartPlannerModuleManager::updateModuleParams( [[maybe_unused]] const std::string ns = name_ + "."; - std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) { - if (!observer.expired()) { - const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); - if (start_planner_ptr) { - start_planner_ptr->updateModuleParams(p); - } + { + updateParam(parameters, ns + "th_arrived_distance", p->th_arrived_distance); + updateParam(parameters, ns + "th_stopped_velocity", p->th_stopped_velocity); + updateParam(parameters, ns + "th_stopped_time", p->th_stopped_time); + updateParam( + parameters, ns + "th_turn_signal_on_lateral_offset", p->th_turn_signal_on_lateral_offset); + updateParam( + parameters, ns + "th_distance_to_middle_of_the_road", p->th_distance_to_middle_of_the_road); + updateParam( + parameters, ns + "intersection_search_length", p->intersection_search_length); + updateParam( + parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection", + p->length_ratio_for_turn_signal_deactivation_near_intersection); + updateParam>( + parameters, ns + "collision_check_margins", p->collision_check_margins); + updateParam( + parameters, ns + "collision_check_margin_from_front_object", + p->collision_check_margin_from_front_object); + updateParam(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity); + updateParam(parameters, ns + "center_line_path_interval", p->center_line_path_interval); + updateParam(parameters, ns + "enable_shift_pull_out", p->enable_shift_pull_out); + updateParam( + parameters, ns + "shift_collision_check_distance_from_end", + p->shift_collision_check_distance_from_end); + updateParam( + parameters, ns + "minimum_shift_pull_out_distance", p->minimum_shift_pull_out_distance); + updateParam( + parameters, ns + "lateral_acceleration_sampling_num", p->lateral_acceleration_sampling_num); + updateParam(parameters, ns + "lateral_jerk", p->lateral_jerk); + updateParam(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc); + updateParam(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc); + updateParam(parameters, ns + "maximum_curvature", p->maximum_curvature); + updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); + updateParam(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); + updateParam(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path); + updateParam( + parameters, ns + "arc_path_interval", p->parallel_parking_parameters.pull_out_path_interval); + updateParam( + parameters, ns + "lane_departure_margin", + p->parallel_parking_parameters.pull_out_lane_departure_margin); + updateParam( + parameters, ns + "pull_out_max_steer_angle", + p->parallel_parking_parameters.pull_out_max_steer_angle); + updateParam(parameters, ns + "enable_back", p->enable_back); + updateParam(parameters, ns + "backward_velocity", p->backward_velocity); + updateParam( + parameters, ns + "geometric_pull_out_velocity", + p->parallel_parking_parameters.pull_out_velocity); + updateParam( + parameters, ns + "geometric_collision_check_distance_from_end", + p->geometric_collision_check_distance_from_end); + updateParam( + parameters, ns + "check_shift_path_lane_departure", p->check_shift_path_lane_departure); + updateParam( + parameters, ns + "allow_check_shift_path_lane_departure_override", + p->allow_check_shift_path_lane_departure_override); + updateParam(parameters, ns + "search_priority", p->search_priority); + updateParam(parameters, ns + "max_back_distance", p->max_back_distance); + updateParam( + parameters, ns + "backward_search_resolution", p->backward_search_resolution); + updateParam( + parameters, ns + "backward_path_update_duration", p->backward_path_update_duration); + updateParam( + parameters, ns + "ignore_distance_from_lane_end", p->ignore_distance_from_lane_end); + } + { + const std::string ns = "start_planner.freespace_planner."; + + updateParam(parameters, ns + "enable_freespace_planner", p->enable_freespace_planner); + updateParam( + parameters, ns + "freespace_planner_algorithm", p->freespace_planner_algorithm); + updateParam( + parameters, ns + "end_pose_search_start_distance", p->end_pose_search_start_distance); + updateParam( + parameters, ns + "end_pose_search_end_distance", p->end_pose_search_end_distance); + updateParam(parameters, ns + "end_pose_search_interval", p->end_pose_search_interval); + updateParam(parameters, ns + "velocity", p->freespace_planner_velocity); + updateParam(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); + updateParam( + parameters, ns + "time_limit", p->freespace_planner_common_parameters.time_limit); + updateParam( + parameters, ns + "minimum_turning_radius", + p->freespace_planner_common_parameters.minimum_turning_radius); + updateParam( + parameters, ns + "maximum_turning_radius", + p->freespace_planner_common_parameters.maximum_turning_radius); + updateParam( + parameters, ns + "turning_radius_size", + p->freespace_planner_common_parameters.turning_radius_size); + p->freespace_planner_common_parameters.maximum_turning_radius = std::max( + p->freespace_planner_common_parameters.maximum_turning_radius, + p->freespace_planner_common_parameters.minimum_turning_radius); + p->freespace_planner_common_parameters.turning_radius_size = + std::max(p->freespace_planner_common_parameters.turning_radius_size, 1); + } + { + const std::string ns = "start_planner.freespace_planner.search_configs."; + + updateParam( + parameters, ns + "theta_size", p->freespace_planner_common_parameters.theta_size); + updateParam( + parameters, ns + "angle_goal_range", p->freespace_planner_common_parameters.angle_goal_range); + updateParam( + parameters, ns + "curve_weight", p->freespace_planner_common_parameters.curve_weight); + updateParam( + parameters, ns + "reverse_weight", p->freespace_planner_common_parameters.reverse_weight); + updateParam( + parameters, ns + "lateral_goal_range", + p->freespace_planner_common_parameters.lateral_goal_range); + updateParam( + parameters, ns + "longitudinal_goal_range", + p->freespace_planner_common_parameters.longitudinal_goal_range); + } + + { + const std::string ns = "start_planner.freespace_planner.costmap_configs."; + + updateParam( + parameters, ns + "obstacle_threshold", + p->freespace_planner_common_parameters.obstacle_threshold); + } + + { + const std::string ns = "start_planner.freespace_planner.astar."; + + updateParam(parameters, ns + "use_back", p->astar_parameters.use_back); + updateParam( + parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions); + updateParam( + parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight); + } + { + const std::string ns = "start_planner.freespace_planner.rrtstar."; + + updateParam(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); + updateParam( + parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling); + updateParam( + parameters, ns + "max_planning_time", p->rrt_star_parameters.max_planning_time); + updateParam(parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius); + updateParam(parameters, ns + "margin", p->rrt_star_parameters.margin); + } + + { + updateParam( + parameters, ns + "stop_condition.maximum_deceleration_for_stop", + p->maximum_deceleration_for_stop); + updateParam( + parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); + } + + const std::string base_ns = "start_planner.path_safety_check."; + const std::string ego_path_ns = base_ns + "ego_predicted_path."; + + { + updateParam( + parameters, ego_path_ns + "min_velocity", p->ego_predicted_path_params.min_velocity); + updateParam( + parameters, ego_path_ns + "min_acceleration", p->ego_predicted_path_params.acceleration); + updateParam( + parameters, ego_path_ns + "time_horizon_for_front_object", + p->ego_predicted_path_params.time_horizon_for_front_object); + updateParam( + parameters, ego_path_ns + "time_horizon_for_rear_object", + p->ego_predicted_path_params.time_horizon_for_rear_object); + updateParam( + parameters, ego_path_ns + "time_resolution", p->ego_predicted_path_params.time_resolution); + updateParam( + parameters, ego_path_ns + "delay_until_departure", + p->ego_predicted_path_params.delay_until_departure); + } + + const std::string obj_filter_ns = base_ns + "target_filtering."; + + { + updateParam( + parameters, obj_filter_ns + "safety_check_time_horizon", + p->objects_filtering_params.safety_check_time_horizon); + updateParam( + parameters, obj_filter_ns + "safety_check_time_resolution", + p->objects_filtering_params.safety_check_time_resolution); + updateParam( + parameters, obj_filter_ns + "object_check_forward_distance", + p->objects_filtering_params.object_check_forward_distance); + updateParam( + parameters, obj_filter_ns + "object_check_backward_distance", + p->objects_filtering_params.object_check_backward_distance); + updateParam( + parameters, obj_filter_ns + "ignore_object_velocity_threshold", + p->objects_filtering_params.ignore_object_velocity_threshold); + updateParam( + parameters, obj_filter_ns + "include_opposite_lane", + p->objects_filtering_params.include_opposite_lane); + updateParam( + parameters, obj_filter_ns + "invert_opposite_lane", + p->objects_filtering_params.invert_opposite_lane); + updateParam( + parameters, obj_filter_ns + "check_all_predicted_path", + p->objects_filtering_params.check_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_all_predicted_path", + p->objects_filtering_params.use_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_predicted_path_outside_lanelet", + p->objects_filtering_params.use_predicted_path_outside_lanelet); + } + + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + + { + updateParam( + parameters, obj_types_ns + "check_car", + p->objects_filtering_params.object_types_to_check.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->objects_filtering_params.object_types_to_check.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->objects_filtering_params.object_types_to_check.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->objects_filtering_params.object_types_to_check.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->objects_filtering_params.object_types_to_check.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->objects_filtering_params.object_types_to_check.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->objects_filtering_params.object_types_to_check.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->objects_filtering_params.object_types_to_check.check_pedestrian); + } + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + + { + updateParam( + parameters, obj_lane_ns + "check_current_lane", + p->objects_filtering_params.object_lane_configuration.check_current_lane); + updateParam( + parameters, obj_lane_ns + "check_right_side_lane", + p->objects_filtering_params.object_lane_configuration.check_right_lane); + updateParam( + parameters, obj_lane_ns + "check_left_side_lane", + p->objects_filtering_params.object_lane_configuration.check_left_lane); + updateParam( + parameters, obj_lane_ns + "check_shoulder_lane", + p->objects_filtering_params.object_lane_configuration.check_shoulder_lane); + updateParam( + parameters, obj_lane_ns + "check_other_lane", + p->objects_filtering_params.object_lane_configuration.check_other_lane); + } + const std::string safety_check_ns = base_ns + "safety_check_params."; + { + updateParam( + parameters, safety_check_ns + "enable_safety_check", + p->safety_check_params.enable_safety_check); + updateParam( + parameters, safety_check_ns + "hysteresis_factor_expand_rate", + p->safety_check_params.hysteresis_factor_expand_rate); + updateParam( + parameters, safety_check_ns + "backward_path_length", + p->safety_check_params.backward_path_length); + updateParam( + parameters, safety_check_ns + "forward_path_length", + p->safety_check_params.forward_path_length); + updateParam( + parameters, safety_check_ns + "publish_debug_marker", + p->safety_check_params.publish_debug_marker); + } + const std::string rss_ns = safety_check_ns + "rss_params."; + { + updateParam( + parameters, rss_ns + "rear_vehicle_reaction_time", + p->safety_check_params.rss_params.rear_vehicle_reaction_time); + updateParam( + parameters, rss_ns + "rear_vehicle_safety_time_margin", + p->safety_check_params.rss_params.rear_vehicle_safety_time_margin); + updateParam( + parameters, rss_ns + "lateral_distance_max_threshold", + p->safety_check_params.rss_params.lateral_distance_max_threshold); + updateParam( + parameters, rss_ns + "longitudinal_distance_min_threshold", + p->safety_check_params.rss_params.longitudinal_distance_min_threshold); + updateParam( + parameters, rss_ns + "longitudinal_velocity_delta_time", + p->safety_check_params.rss_params.longitudinal_velocity_delta_time); + } + std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; + { + updateParam( + parameters, surround_moving_obstacle_check_ns + "search_radius", p->search_radius); + updateParam( + parameters, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity", + p->th_moving_obstacle_velocity); + + // ObjectTypesToCheck + std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + { + updateParam( + parameters, obj_types_ns + "check_car", + p->surround_moving_obstacles_type_to_check.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->surround_moving_obstacles_type_to_check.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->surround_moving_obstacles_type_to_check.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->surround_moving_obstacles_type_to_check.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->surround_moving_obstacles_type_to_check.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->surround_moving_obstacles_type_to_check.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->surround_moving_obstacles_type_to_check.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->surround_moving_obstacles_type_to_check.check_pedestrian); } - }); + } } bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 1d86018c424dd..d9e67af74dbeb 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -79,13 +79,27 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points.begin() + pull_out_end_idx + 1); } - // check lane departure - // The method for lane departure checking verifies if the footprint of each point on the path is - // contained within a lanelet using `boost::geometry::within`, which incurs a high computational - // cost. const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + + // if lane departure check override is true, and if the initial pose is not fully within a lane, + // cancel lane departure check + const bool is_lane_departure_check_required = std::invoke([&]() -> bool { + if (!parameters_.allow_check_shift_path_lane_departure_override) + return parameters_.check_shift_path_lane_departure; + + PathWithLaneId path_with_only_first_pose{}; + path_with_only_first_pose.points.push_back(path_shift_start_to_end.points.at(0)); + return !lane_departure_checker_->checkPathWillLeaveLane( + lanelet_map_ptr, path_with_only_first_pose); + }); + + // check lane departure + // The method for lane departure checking verifies if the footprint of each point on the path + // is contained within a lanelet using `boost::geometry::within`, which incurs a high + // computational cost. + if ( - parameters_.check_shift_path_lane_departure && + is_lane_departure_check_required && lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) { continue; } diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 83d2b5cd2dab1..6f092d0a351e4 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -175,15 +175,10 @@ void StartPlannerModule::updateData() if (hasFinishedBackwardDriving()) { updateStatusAfterBackwardDriving(); DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving"); - } else { - status_.backward_driving_complete = false; } - if (requiresDynamicObjectsCollisionDetection()) { - status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects(); - } else { - status_.is_safe_dynamic_objects = true; - } + status_.is_safe_dynamic_objects = + (!requiresDynamicObjectsCollisionDetection()) ? true : !hasCollisionWithDynamicObjects(); } bool StartPlannerModule::hasFinishedBackwardDriving() const @@ -322,20 +317,17 @@ bool StartPlannerModule::isStopped() bool StartPlannerModule::isExecutionReady() const { - bool is_safe = true; // Evaluate safety. The situation is not safe if any of the following conditions are met: // 1. pull out path has not been found // 2. there is a moving objects around ego // 3. waiting for approval and there is a collision with dynamic objects - if (!status_.found_pull_out_path) { - is_safe = false; - } else if (isWaitingApproval()) { - if (!noMovingObjectsAround()) { - is_safe = false; - } else if (requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()) { - is_safe = false; - } - } + + const bool is_safe = [&]() -> bool { + if (!status_.found_pull_out_path) return false; + if (!isWaitingApproval()) return true; + if (!noMovingObjectsAround()) return false; + return !(requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()); + }(); if (!is_safe) { stop_pose_ = planner_data_->self_odometry->pose.pose; @@ -417,14 +409,7 @@ BehaviorModuleOutput StartPlannerModule::plan() setDrivableAreaInfo(output); - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::STRAIGHT; - }); + const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( @@ -438,15 +423,16 @@ BehaviorModuleOutput StartPlannerModule::plan() {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); - } else { - const double distance = motion_utils::calcSignedArcLength( - path.points, planner_data_->self_odometry->pose.pose.position, - status_.pull_out_path.start_pose.position); - updateRTCStatus(0.0, distance); - steering_factor_interface_ptr_->updateSteeringFactor( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); + setDebugData(); + return output; } + const double distance = motion_utils::calcSignedArcLength( + path.points, planner_data_->self_odometry->pose.pose.position, + status_.pull_out_path.start_pose.position); + updateRTCStatus(0.0, distance); + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, + PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); setDebugData(); @@ -527,14 +513,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() setDrivableAreaInfo(output); - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::STRAIGHT; - }); + const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( @@ -548,15 +527,17 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); - } else { - const double distance = motion_utils::calcSignedArcLength( - stop_path.points, planner_data_->self_odometry->pose.pose.position, - status_.pull_out_path.start_pose.position); - updateRTCStatus(0.0, distance); - steering_factor_interface_ptr_->updateSteeringFactor( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); + setDebugData(); + + return output; } + const double distance = motion_utils::calcSignedArcLength( + stop_path.points, planner_data_->self_odometry->pose.pose.position, + status_.pull_out_path.start_pose.position); + updateRTCStatus(0.0, distance); + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, + PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); setDebugData(); @@ -616,16 +597,20 @@ PriorityOrder StartPlannerModule::determinePriorityOrder( order_priority.emplace_back(i, planner); } } - } else if (search_priority == "short_back_distance") { + return order_priority; + } + + if (search_priority == "short_back_distance") { for (size_t i = 0; i < start_pose_candidates_num; i++) { for (const auto & planner : start_planners_) { order_priority.emplace_back(i, planner); } } - } else { - RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str()); - throw std::domain_error("[start_planner] invalid search_priority"); + return order_priority; } + + RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str()); + throw std::domain_error("[start_planner] invalid search_priority"); return order_priority; } @@ -1047,12 +1032,7 @@ bool StartPlannerModule::isStuck() return false; } - if (status_.planner_type == PlannerType::STOP) { - return true; - } - - // not found safe path - if (!status_.found_pull_out_path) { + if (status_.planner_type == PlannerType::STOP || !status_.found_pull_out_path) { return true; } @@ -1145,30 +1125,26 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return false; }); - if (is_near_intersection) { - // offset required end pose with ration to activate turn signal for intersection - turn_signal.required_end_point = std::invoke([&]() { - const double length_start_to_end = - motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position); - const auto ratio = std::clamp( - parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0); - - const double required_end_length = length_start_to_end * ratio; - double accumulated_length = 0.0; - const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - for (size_t i = start_idx; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > required_end_length) { - return path.points.at(i).point.pose; - } + turn_signal.required_end_point = std::invoke([&]() { + if (is_near_intersection) return end_pose; + const double length_start_to_end = + motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position); + const auto ratio = std::clamp( + parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0); + + const double required_end_length = length_start_to_end * ratio; + double accumulated_length = 0.0; + const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); + for (size_t i = start_idx; i < path.points.size() - 1; ++i) { + accumulated_length += + tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + if (accumulated_length > required_end_length) { + return path.points.at(i).point.pose; } - // not found required end point - return end_pose; - }); - } else { - turn_signal.required_end_point = end_pose; - } + } + // not found required end point + return end_pose; + }); return turn_signal; } @@ -1334,23 +1310,27 @@ bool StartPlannerModule::planFreespacePath() void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { - if (status_.planner_type == PlannerType::FREESPACE) { - std::cerr << "Freespace planner updated drivable area." << std::endl; - const double drivable_area_margin = planner_data_->parameters.vehicle_width; - output.drivable_area_info.drivable_margin = - planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; - } else { - const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - output.path, generateDrivableLanes(output.path), - planner_data_->drivable_area_expansion_parameters); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = target_drivable_lanes; - output.drivable_area_info = - status_.driving_forward - ? utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) - : current_drivable_area_info; + switch (status_.planner_type) { + case PlannerType::FREESPACE: { + const double drivable_area_margin = planner_data_->parameters.vehicle_width; + output.drivable_area_info.drivable_margin = + planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; + break; + } + default: { + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + output.path, generateDrivableLanes(output.path), + planner_data_->drivable_area_expansion_parameters); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = + status_.driving_forward + ? utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) + : current_drivable_area_info; + break; + } } }