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 2d5da0f070ed9..5b8aca1c8ec9d 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 @@ -44,8 +44,6 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using autoware::motion_utils::VelocityFactorInterface; - IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, 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 f32210bc502bf..0e646e92035cf 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 @@ -38,7 +38,6 @@ NoDrivableLaneModule::NoDrivableLaneModule( debug_data_(), state_(State::INIT) { - velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE); } bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path) @@ -166,8 +165,10 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = op_stop_pose.value(); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + 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*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -214,8 +215,10 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * // Get stop point and stop factor { const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + 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*/, ""); const auto & virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -252,8 +255,10 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = ego_pos_on_path.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); + 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*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 1163b00e08ccb..fa259b2bea9aa 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -70,8 +70,6 @@ OcclusionSpotModule::OcclusionSpotModule( : SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), param_(planner_param) { - velocity_factor_.init(PlanningBehavior::UNKNOWN); - if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; //! occupancy grid limitation( 100 * 100 ) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index cb57974ed6700..5c0ba7809b603 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -28,8 +27,6 @@ #include #include -#include -#include #include #include #include @@ -51,8 +48,6 @@ namespace autoware::behavior_velocity_planner { -using autoware::motion_utils::PlanningBehavior; -using autoware::motion_utils::VelocityFactor; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::universe_utils::DebugPublisher; @@ -97,8 +92,6 @@ class SceneModuleInterface planner_data_ = planner_data; } - void resetVelocityFactor() { velocity_factor_.reset(); } - VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } std::vector getObjectsOfInterestData() const { return objects_of_interest_; } void clearObjectsOfInterestData() { objects_of_interest_.clear(); } @@ -107,7 +100,6 @@ class SceneModuleInterface rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - autoware::motion_utils::VelocityFactorInterface velocity_factor_; // TODO(soblin): remove this std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; std::shared_ptr planning_factor_interface_; @@ -143,8 +135,6 @@ class SceneModuleManagerInterface } pub_virtual_wall_ = node.create_publisher( std::string("~/virtual_wall/") + module_name, 5); - pub_velocity_factor_ = node.create_publisher( - std::string("/planning/velocity_factors/") + module_name, 1); planning_factor_interface_ = std::make_shared(&node, module_name); @@ -180,22 +170,11 @@ class SceneModuleManagerInterface StopWatch stop_watch; stop_watch.tic("Total"); visualization_msgs::msg::MarkerArray debug_marker_array; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); for (const auto & scene_module : scene_modules_) { - scene_module->resetVelocityFactor(); scene_module->setPlannerData(planner_data_); scene_module->modifyPathVelocity(path); - // The velocity factor must be called after modifyPathVelocity. - // TODO(soblin): remove this - const auto velocity_factor = scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { debug_marker_array.markers.push_back(marker); } @@ -203,7 +182,6 @@ class SceneModuleManagerInterface virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); } - pub_velocity_factor_->publish(velocity_factor_array); planning_factor_interface_->publish(); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { @@ -273,8 +251,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::Publisher::SharedPtr pub_debug_path_; - rclcpp::Publisher::SharedPtr - pub_velocity_factor_; std::shared_ptr processing_time_publisher_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index 9a402f31af0e4..aa4c9b1ba5fd0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -61,7 +61,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat for (const auto & scene_module : scene_modules_) { std::shared_ptr traffic_light_scene_module( std::dynamic_pointer_cast(scene_module)); - traffic_light_scene_module->resetVelocityFactor(); traffic_light_scene_module->setPlannerData(planner_data_); traffic_light_scene_module->modifyPathVelocity(path);