diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 64e1435167a89..a978c074e0018 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -101,8 +101,11 @@ void BlindSpotModule::reactRTCApprovalByDecision( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, + "blind_spot(module is judging as UNSAFE)"); } return; } @@ -132,8 +135,11 @@ void BlindSpotModule::reactRTCApprovalByDecision( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, + "blind_spot(module is judging as SAFE and RTC is not approved)"); } return; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 6e1f07712d9af..4fca5f821f934 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -49,7 +49,6 @@ BlindSpotModule::BlindSpotModule( turn_direction_(turn_direction), is_over_pass_judge_line_(false) { - velocity_factor_.init(PlanningBehavior::REAR_CHECK); sibling_straight_lanelet_ = getSiblingStraightLanelet( planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_), planner_data->route_handler_->getRoutingGraphPtr()); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 09fe8db600229..2d5da0f070ed9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -18,7 +18,6 @@ #include // for toGeomPoly #include -#include #include #include // for toPolygon2d #include @@ -63,8 +62,6 @@ IntersectionModule::IntersectionModule( has_traffic_light_(has_traffic_light), occlusion_uuid_(autoware::universe_utils::generateUUID()) { - velocity_factor_.init(PlanningBehavior::INTERSECTION); - { collision_state_machine_.setMarginTime( planner_param_.collision_detection.collision_detection_hold_time); @@ -707,7 +704,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); @@ -722,7 +719,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -736,7 +733,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] motion_utils::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -748,7 +745,8 @@ void reactRTCApprovalByDecisionResult( const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -762,9 +760,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(pure StuckStop)"); } } if (!rtc_occlusion_approved && decision_result.occlusion_stopline_idx) { @@ -773,9 +774,13 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(occlusion_stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(occlusion_stopline_idx).point.pose, + path->points.at(occlusion_stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(StuckStop with occlusion)"); } } return; @@ -787,7 +792,8 @@ void reactRTCApprovalByDecisionResult( const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -801,9 +807,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Yield Stuck)"); } } return; @@ -815,7 +824,8 @@ void reactRTCApprovalByDecisionResult( const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -827,9 +837,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop)"); } } if (!rtc_occlusion_approved) { @@ -838,9 +851,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } return; @@ -851,7 +867,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + tier4_planning_msgs::msg::PathWithLaneId * path, + motion_utils::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -864,9 +881,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_first_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with collision)"); } } if (!rtc_occlusion_approved) { @@ -883,9 +903,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with occlusion)"); } } return; @@ -896,7 +919,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + tier4_planning_msgs::msg::PathWithLaneId * path, + motion_utils::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -922,9 +946,13 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(occlusion_peeking_stopline).point.pose, VelocityFactor::UNKNOWN); + path->points.at(occlusion_peeking_stopline).point.pose, + path->points.at(occlusion_peeking_stopline).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(PeekingToOcclusion)"); } } if (!rtc_default_approved) { @@ -933,9 +961,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(PeekingToOcclusion while stopping for collision)"); } } return; @@ -947,7 +978,8 @@ void reactRTCApprovalByDecisionResult( const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -959,9 +991,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } if (!rtc_occlusion_approved) { @@ -974,9 +1009,12 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } return; @@ -988,7 +1026,8 @@ void reactRTCApprovalByDecisionResult( const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1000,9 +1039,13 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, + "intersection(Occlusion without traffic light, collision detected)"); } } if (!rtc_occlusion_approved && decision_result.temporal_stop_before_attention_required) { @@ -1011,9 +1054,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Occlusion without traffic light)"); } } if (!rtc_occlusion_approved && !decision_result.temporal_stop_before_attention_required) { @@ -1034,7 +1080,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1045,9 +1092,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for collision)"); } } if (!rtc_occlusion_approved) { @@ -1056,9 +1106,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for occlusion)"); } } return; @@ -1070,7 +1123,8 @@ void reactRTCApprovalByDecisionResult( const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + motion_utils::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1082,9 +1136,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FullyPrioritized, collision detected)"); } } if (!rtc_occlusion_approved) { @@ -1093,9 +1150,13 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, + "intersection(FullyPrioritized, RTC for occlusion is interrupting)"); } } return; @@ -1109,7 +1170,7 @@ void IntersectionModule::reactRTCApproval( VisitorSwitch{[&](const auto & decision) { reactRTCApprovalByDecisionResult( activated_, occlusion_activated_, decision, planner_param_, baselink2front, path, - &velocity_factor_, &debug_data_); + planning_factor_interface_.get(), &debug_data_); }}, decision_result); return; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 2392817aebaf2..fc442337198af 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -48,7 +48,6 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( lane_id_(lane_id), associative_ids_(associative_ids) { - velocity_factor_.init(PlanningBehavior::MERGE); planner_param_ = planner_param; state_machine_.setState(StateMachine::State::STOP); } @@ -155,8 +154,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path) /* get stop point and stop factor */ const auto & stop_pose = path->points.at(stopline_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "merge_from_private"); const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength( path->points, current_pose.position, path->points.at(stopline_idx).point.pose.position);