From 701594965c4228d2965eec896bcc19d17370a0af Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 15 Jan 2025 11:48:51 +0900 Subject: [PATCH] Feat/development branch planning factor bvp crosswalk (#1760) add planning_factor_interface to behavior_velocity_planner_crosswalk Signed-off-by: Mamoru Sobue --- .../src/scene_crosswalk.cpp | 15 +++++++++------ .../src/scene_walkway.cpp | 9 ++++----- 2 files changed, 13 insertions(+), 11 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 7c9035d135065..844647e33f8c0 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 @@ -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/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 c60c3def50270..94421ae27e077 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 @@ -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( 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 =