fix(behavior_velocity_planner): planning factor integration

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)");

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");
 }

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*/, "");

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -1,4 +1,4 @@
 // Copyright 2020 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -764,7 +764,7 @@
     {
       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 @@
       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 @@
     {
       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 @@
     {
       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 @@
     {
       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 @@
     {
       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 @@
     {
       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 @@
       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 @@
     {
       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 @@
     {
       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 @@
     {
       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 @@
     {
       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 @@
     {
       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 @@
     {
       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 @@
     {
       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 @@
     {
       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 @@
     {
       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*/,

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");

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*/, "");

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*/, "");

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");
 

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");

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");

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*/, "");

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");