add planning_factor_interface to behavior_velocity_planner_crosswalk

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

Hide file tree

Showing 2 changed files with 13 additions and 11 deletions.
diff --git a/...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/...vior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp
@@ -183,7 +183,6 @@ CrosswalkModule::CrosswalkModule(
   planner_param_(planner_param),
   use_regulatory_element_(reg_elem_id)
 {
-  velocity_factor_.init(PlanningBehavior::CROSSWALK);
   passed_safety_slow_point_ = false;
 
   if (use_regulatory_element_) {
@@ -900,9 +899,11 @@ void CrosswalkModule::applySlowDown(
     }
   }
   if (slowdown_pose)
-    velocity_factor_.set(
-      output.points, planner_data_->current_odometry->pose, *slowdown_pose,
-      VelocityFactor::APPROACHING);
+    planning_factor_interface_->add(
+      output.points, planner_data_->current_odometry->pose, *slowdown_pose, *slowdown_pose,
+      tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN,
+      tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/,
+      safety_slow_down_speed, 0.0 /*shift distance*/, "crosswalk_safety_slowdown_for_approaching");
 }
 
 void CrosswalkModule::applySlowDownByLanelet2Map(
@@ -1379,9 +1380,11 @@ void CrosswalkModule::planStop(
 
   // Plan stop
   insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path);
-  velocity_factor_.set(
+  planning_factor_interface_->add(
     ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose,
-    VelocityFactor::UNKNOWN);
+    stop_factor->stop_pose, tier4_planning_msgs::msg::PlanningFactor::STOP,
+    tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/,
+    0.0 /*shift distance*/, "crosswalk_stop");
 }
 
 bool CrosswalkModule::checkRestartSuppression(

diff --git a/...behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/...behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp
@@ -42,8 +42,6 @@ WalkwayModule::WalkwayModule(
   planner_param_(planner_param),
   use_regulatory_element_(use_regulatory_element)
 {
-  velocity_factor_.init(PlanningBehavior::SIDEWALK);
-
   if (use_regulatory_element_) {
     const auto reg_elem_ptr = std::dynamic_pointer_cast<const lanelet::autoware::Crosswalk>(
       lanelet_map_ptr->regulatoryElementLayer.get(module_id));
@@ -122,9 +120,10 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path)
     }
 
     /* get stop point and stop factor */
-    velocity_factor_.set(
-      path->points, planner_data_->current_odometry->pose, stop_pose.value(),
-      VelocityFactor::UNKNOWN);
+    planning_factor_interface_->add(
+      path->points, planner_data_->current_odometry->pose, stop_pose.value(), stop_pose.value(),
+      tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
+      true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift distance*/, "walkway_stop");
 
     // use arc length to identify if ego vehicle is in front of walkway stop or not.
     const double signed_arc_dist_to_stop_point =