From 67ab350f6e1af0c14bee74d6913c99165c2e9262 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 17 Jan 2025 00:05:32 +0900 Subject: [PATCH] feat(behavior_velocity_planner)!: remove velocity_factor completely (#9943) * feat(behavior_velocity_planner)!: remove velocity_factor completely Signed-off-by: Mamoru Sobue * minimize diff Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../src/scene.cpp | 8 +++--- .../src/scene_intersection.cpp | 2 -- .../src/scene.cpp | 19 +++++++++----- .../src/scene_no_stopping_area.cpp | 9 ++++--- .../src/scene_occlusion_spot.cpp | 2 -- .../scene_module_interface.hpp | 22 ---------------- .../src/scene.cpp | 26 +++++-------------- .../src/scene.hpp | 5 ---- .../src/manager.cpp | 12 --------- .../src/scene.cpp | 7 ++--- .../src/scene.cpp | 15 ++++++----- .../src/scene.hpp | 4 --- 12 files changed, 39 insertions(+), 92 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 59f6f20937f41..7e6a320f3ddad 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -46,7 +46,6 @@ DetectionAreaModule::DetectionAreaModule( planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) @@ -180,9 +179,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - 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*/, ""); } return true; 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_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index bcb8b365f6205..c788a54073ed6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -50,7 +50,6 @@ NoStoppingAreaModule::NoStoppingAreaModule( planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA); state_machine_.setState(StateMachine::State::GO); state_machine_.setMarginTime(planner_param_.state_clear_time); } @@ -144,9 +143,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_point->second, stop_point->second, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); } } else if (state_machine_.getState() == StateMachine::State::GO) { 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 ad1ae1d47664a..5bff3ca343e72 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,21 +170,12 @@ 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 +184,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_) { @@ -274,8 +254,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_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 71a5ea3b9e807..e2ddbc75c7b57 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -38,7 +38,6 @@ StopLineModule::StopLineModule( state_(State::APPROACH), debug_data_() { - velocity_factor_.init(PlanningBehavior::STOP_SIGN); } bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) @@ -62,7 +61,12 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) path->points = trajectory->restore(); - updateVelocityFactor(&velocity_factor_, state_, *stop_point - ego_s); + // TODO(soblin): PlanningFactorInterface use trajectory class + planning_factor_interface_->add( + path->points, trajectory->compute(*stop_point).point.pose, + planner_data_->current_odometry->pose, planner_data_->current_odometry->pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); updateStateAndStoppedTime( &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); @@ -152,24 +156,6 @@ void StopLineModule::updateStateAndStoppedTime( } } -void StopLineModule::updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point) -{ - switch (state) { - case State::APPROACH: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::APPROACHING); - break; - } - case State::STOPPED: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::STOPPED); - break; - } - case State::START: - break; - } -} - void StopLineModule::updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index aac830b0e9f24..7d430463c18b9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -18,7 +18,6 @@ #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" -#include "autoware/motion_utils/factor/velocity_factor_interface.hpp" #include "autoware/trajectory/path_point_with_lane_id.hpp" #include @@ -98,10 +97,6 @@ class StopLineModule : public SceneModuleInterface State * state, std::optional * stopped_time, const rclcpp::Time & now, const double & distance_to_stop_point, const bool & is_vehicle_stopped) const; - static void updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point); - void updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const; 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..1d1ee7fc996b2 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 @@ -53,24 +53,13 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat autoware_perception_msgs::msg::TrafficLightGroup tl_state; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); 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); - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = traffic_light_scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < nearest_ref_stop_path_point_index_) { @@ -88,7 +77,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat virtual_wall_marker_creator_.add_virtual_walls( traffic_light_scene_module->createVirtualWalls()); } - pub_velocity_factor_->publish(velocity_factor_array); pub_debug_->publish(debug_marker_array); pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); pub_tl_state_->publish(tl_state); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index b051d5ff7eb76..becbaf5ee05f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -54,7 +54,6 @@ TrafficLightModule::TrafficLightModule( debug_data_(), is_prev_state_stop_(false) { - velocity_factor_.init(PlanningBehavior::TRAFFIC_SIGNAL); planner_param_ = planner_param; } @@ -293,9 +292,11 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( size_t insert_index = insert_target_point_idx; planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index); - velocity_factor_.set( + planning_factor_interface_->add( modified_path.points, planner_data_->current_odometry->pose, - target_point_with_lane_id.point.pose, VelocityFactor::UNKNOWN); + target_point_with_lane_id.point.pose, target_point_with_lane_id.point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); return modified_path; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 9ba7b0bccf1b9..a5779cfc0712a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -48,8 +48,6 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( lane_(lane), planner_param_(planner_param) { - velocity_factor_.init(PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT); - // Get map data const auto instrument = reg_elem_.getVirtualTrafficLight(); const auto instrument_bottom_line = toAutowarePoints(instrument); @@ -420,9 +418,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN, - command_.type); + 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*/, ""); // Set data for visualization module_data_.stop_head_pose_at_stop_line = @@ -453,8 +452,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( } // Set StopReason - 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*/, ""); // Set data for visualization module_data_.stop_head_pose_at_end_line = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index 9e7a1192d9869..0661267b3d361 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -113,10 +113,6 @@ class VirtualTrafficLightModule : public SceneModuleInterface void updateInfrastructureCommand(); - void setVelocityFactor( - const geometry_msgs::msg::Pose & stop_pose, - autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor); - std::optional getPathIndexOfFirstEndLine(); bool isBeforeStartLine(const size_t end_line_idx);