From 1722bc637bc1fa89e484226bae3982690de4177d Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:04:42 +0900 Subject: [PATCH 01/11] fix: blind_spot Signed-off-by: satoshi-ota --- .../src/decisions.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 0dac946353634..69b65397734ee 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 @@ -102,7 +102,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + path->points, planner_data_->current_odometry->pose, stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "blind_spot(module is judging as UNSAFE)"); @@ -136,7 +136,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + path->points, planner_data_->current_odometry->pose, stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_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)"); From 229a6be031dfb86efe015ecea7726bfb54d6349b Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:04:54 +0900 Subject: [PATCH 02/11] fix: crosswalk Signed-off-by: satoshi-ota --- .../src/scene_crosswalk.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 5face5aa6035a..7ae6e16bb21e1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -881,7 +881,7 @@ void CrosswalkModule::applySlowDown( } if (slowdown_pose) planning_factor_interface_->add( - output.points, planner_data_->current_odometry->pose, *slowdown_pose, *slowdown_pose, + output.points, planner_data_->current_odometry->pose, *slowdown_pose, autoware_internal_planning_msgs::msg::PlanningFactor::SLOW_DOWN, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, safety_slow_down_speed, 0.0 /*shift distance*/, "crosswalk_safety_slowdown_for_approaching"); @@ -1365,7 +1365,7 @@ void CrosswalkModule::planStop( insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path); planning_factor_interface_->add( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, - stop_factor->stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift distance*/, "crosswalk_stop"); } From e4b7585c8ce10ae106ba64a1fbb08b365b75e08a Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:05:07 +0900 Subject: [PATCH 03/11] fix: detection_area Signed-off-by: satoshi-ota --- .../src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 14220d1b630af..35568fd789e64 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -181,7 +181,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + path->points, planner_data_->current_odometry->pose, stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); From 4d1c2105bfff4fa7df933dd706e979cfba5a376f Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:05:18 +0900 Subject: [PATCH 04/11] fix: intersection Signed-off-by: satoshi-ota --- .../src/scene_intersection.cpp | 32 +++++++++---------- .../src/scene_merge_from_private_road.cpp | 2 +- 2 files changed, 16 insertions(+), 18 deletions(-) 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 81013615a5f79..55fd6da842577 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 @@ -764,7 +764,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(pure StuckStop)"); @@ -779,7 +779,6 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, path->points.at(occlusion_stopline_idx).point.pose, - path->points.at(occlusion_stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(StuckStop with occlusion)"); @@ -811,7 +810,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(Yield Stuck)"); @@ -841,7 +840,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(CollisionStop)"); @@ -855,7 +854,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); @@ -885,7 +884,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with collision)"); @@ -907,7 +906,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with occlusion)"); @@ -951,7 +950,6 @@ void reactRTCApprovalByDecisionResult( planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(occlusion_peeking_stopline).point.pose, - path->points.at(occlusion_peeking_stopline).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(PeekingToOcclusion)"); @@ -965,7 +963,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(PeekingToOcclusion while stopping for collision)"); @@ -995,7 +993,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); @@ -1013,7 +1011,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); @@ -1043,7 +1041,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, @@ -1058,7 +1056,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(Occlusion without traffic light)"); @@ -1096,7 +1094,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for collision)"); @@ -1110,7 +1108,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for occlusion)"); @@ -1140,7 +1138,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "intersection(FullyPrioritized, collision detected)"); @@ -1154,7 +1152,7 @@ void reactRTCApprovalByDecisionResult( { planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + path->points.at(stopline_idx).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, 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 22801295511d0..311c1e3a2e921 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 @@ -157,7 +157,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path) /* get stop point and stop factor */ const auto & stop_pose = path->points.at(stopline_idx).point.pose; planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + path->points, planner_data_->current_odometry->pose, stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "merge_from_private"); From 0b059bc798138c6184f621a262ef77703c30c002 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:05:31 +0900 Subject: [PATCH 05/11] fix: no_drivable_lane Signed-off-by: satoshi-ota --- .../src/scene.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index ac66f1d1a6b07..dcf46ad21a161 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -167,7 +167,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) { const auto & stop_pose = op_stop_pose.value(); planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + path->points, planner_data_->current_odometry->pose, stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); @@ -218,7 +218,7 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * { const auto & stop_pose = autoware_utils::get_pose(path->points.at(0)); planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + path->points, planner_data_->current_odometry->pose, stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); @@ -259,7 +259,7 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path) { const auto & stop_pose = ego_pos_on_path.pose; planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + path->points, planner_data_->current_odometry->pose, stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); From f8523678504549e1c76f02b4f2dd204cda1c8719 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:05:42 +0900 Subject: [PATCH 06/11] fix: no_stopping_area Signed-off-by: satoshi-ota --- .../src/scene_no_stopping_area.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index ad39fe532ee4d..c72472ea14e52 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -145,7 +145,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_point->second, stop_point->second, + path->points, planner_data_->current_odometry->pose, stop_point->second, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); From f1fc6089075aab96c8dadd51ab4c3e16ad7dd442 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:05:51 +0900 Subject: [PATCH 07/11] fix: run_out Signed-off-by: satoshi-ota --- .../autoware_behavior_velocity_run_out_module/src/scene.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index 8aa0bd0cdea08..759573e6941d6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -768,7 +768,7 @@ bool RunOutModule::insertStopPoint( planning_utils::insertVelocity(path, stop_point_with_lane_id, stop_point_velocity, insert_idx); planning_factor_interface_->add( - path.points, planner_data_->current_odometry->pose, stop_point.value(), stop_point.value(), + path.points, planner_data_->current_odometry->pose, stop_point.value(), autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift_distance*/, "run_out_stop"); @@ -884,7 +884,7 @@ void RunOutModule::insertApproachingVelocity( planning_factor_interface_->add( output_path.points, planner_data_->current_odometry->pose, stop_point.value(), - stop_point.value(), autoware_internal_planning_msgs::msg::PlanningFactor::STOP, + autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift_distance*/, "run_out_approaching_velocity"); From 01bd5e33fc28c35b43cdf31b15b6430d075f5518 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:06:01 +0900 Subject: [PATCH 08/11] fix: stop_line Signed-off-by: satoshi-ota --- .../autoware_behavior_velocity_stop_line_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index f230e49118756..6062b01375f5a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -67,7 +67,7 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) // TODO(soblin): PlanningFactorInterface use trajectory class planning_factor_interface_->add( path->points, planner_data_->current_odometry->pose, - trajectory->compute(*stop_point).point.pose, trajectory->compute(*stop_point).point.pose, + trajectory->compute(*stop_point).point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); From 372beab1a9fdd367d24d57c5604a9af635ade8f4 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:06:12 +0900 Subject: [PATCH 09/11] fix: traffic_light Signed-off-by: satoshi-ota --- .../src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 2aa25696397e5..3717c5ebfdd4f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -296,7 +296,7 @@ autoware_internal_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertS planning_factor_interface_->add( modified_path.points, planner_data_->current_odometry->pose, - target_point_with_lane_id.point.pose, target_point_with_lane_id.point.pose, + target_point_with_lane_id.point.pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "traffic_light"); From ee668047876e612a7ec73062ce20065b3f452266 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:06:21 +0900 Subject: [PATCH 10/11] fix: virtual_traffic_light Signed-off-by: satoshi-ota --- .../src/scene.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index d2aad40b7a178..0ad9d85ee2808 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -420,7 +420,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( // Set StopReason planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + path->points, planner_data_->current_odometry->pose, stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); @@ -455,7 +455,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( // Set StopReason planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + path->points, planner_data_->current_odometry->pose, stop_pose, autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); From 466e6bf44f885bcdcd10d0bc969b3b6081df3b54 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 18 Mar 2025 14:06:33 +0900 Subject: [PATCH 11/11] fix: walk_way Signed-off-by: satoshi-ota --- .../src/scene_walkway.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 4d8651d05c330..827655daa2717 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -123,7 +123,7 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path) /* get stop point and stop factor */ planning_factor_interface_->add( - path->points, planner_data_->current_odometry->pose, stop_pose.value(), stop_pose.value(), + path->points, planner_data_->current_odometry->pose, stop_pose.value(), autoware_internal_planning_msgs::msg::PlanningFactor::STOP, autoware_internal_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift distance*/, "walkway_stop");