add planning_factor_interface to behavior_velocity_planner_intersections

Signed-off-by: Mamoru Sobue <[email protected]>
tier4 · Jan 15, 2025 · 1fc59d8 · 1fc59d8
1 parent 7015949
commit 1fc59d8
Show file tree

Hide file tree

Showing 4 changed files with 125 additions and 58 deletions.
diff --git a/.../behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/.../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/...ning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/...ning/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());