From 2e3280997a9ae218d23df8825948bf352234a5e8 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 26 Dec 2024 10:14:42 +0900 Subject: [PATCH] fix(behavior_path_planner): remove velocity factor Signed-off-by: satoshi-ota --- .../behavior_path_planner_node.hpp | 4 -- .../src/behavior_path_planner_node.cpp | 28 --------- .../src/planner_manager.cpp | 2 - .../interface/scene_module_interface.hpp | 60 ------------------- .../scene_module_manager_interface.hpp | 48 --------------- .../scene_module_manager_interface.cpp | 4 -- 6 files changed, 146 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index da1e1298f89e3..7f7ecd7326b08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -21,7 +21,6 @@ #include "planner_manager.hpp" #include -#include #include #include @@ -53,7 +52,6 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::PlanningFactorInterface; -using autoware::motion_utils::SteeringFactorInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObject; @@ -123,7 +121,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr reroute_availability_publisher_; - rclcpp::Publisher::SharedPtr pub_steering_factors_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -138,7 +135,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_manager_; - SteeringFactorInterface steering_factor_interface_; std::unique_ptr planning_factor_interface_; std::mutex mutex_pd_; // mutex for planner_data_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index ce0b201ad30d3..568d432e1faa3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -53,8 +53,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const auto durable_qos = rclcpp::QoS(1).transient_local(); modified_goal_publisher_ = create_publisher("~/output/modified_goal", durable_qos); - pub_steering_factors_ = - create_publisher("/planning/steering_factor/intersection", 1); reroute_availability_publisher_ = create_publisher("~/output/is_reroute_available", 1); debug_avoidance_msg_array_publisher_ = @@ -117,8 +115,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod turn_signal_search_time, turn_signal_intersection_angle_threshold_deg); } - steering_factor_interface_.init(PlanningBehavior::INTERSECTION); - // Start timer { const auto planning_hz = declare_parameter("planning_hz"); @@ -465,20 +461,9 @@ void BehaviorPathPlannerNode::publish_steering_factor( const auto [intersection_flag, approaching_intersection_flag] = planner_data->turn_signal_decider.getIntersectionTurnSignalFlag(); if (intersection_flag || approaching_intersection_flag) { - const uint16_t steering_factor_direction = std::invoke([&turn_signal]() { - if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - const auto [intersection_pose, intersection_distance] = planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); - steering_factor_interface_.set( - {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance}, - steering_factor_direction, SteeringFactor::TURNING, ""); - const uint16_t planning_factor_direction = std::invoke([&turn_signal]() { if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { return PlanningFactor::TURN_LEFT; @@ -489,21 +474,8 @@ void BehaviorPathPlannerNode::publish_steering_factor( planning_factor_interface_->add( intersection_distance, intersection_distance, intersection_pose, intersection_pose, planning_factor_direction, SafetyFactorArray{}); - } else { - steering_factor_interface_.reset(); } - autoware_adapi_v1_msgs::msg::SteeringFactorArray steering_factor_array; - steering_factor_array.header.frame_id = "map"; - steering_factor_array.header.stamp = this->now(); - - const auto steering_factor = steering_factor_interface_.get(); - if (steering_factor.behavior != PlanningBehavior::UNKNOWN) { - steering_factor_array.factors.emplace_back(steering_factor); - } - - pub_steering_factors_->publish(steering_factor_array); - planning_factor_interface_->publish(); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 372893d1b1a45..322209d46732b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -182,8 +182,6 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); m->publishRTCStatus(); - m->publishSteeringFactor(); - m->publishVelocityFactor(); m->publish_planning_factors(); }); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 4e9f433f11469..c96bc601b6403 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -22,8 +22,6 @@ #include #include -#include -#include #include #include #include @@ -37,7 +35,6 @@ #include #include -#include #include #include #include @@ -56,16 +53,11 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::PlanningFactorInterface; -using autoware::motion_utils::SteeringFactorInterface; -using autoware::motion_utils::VelocityFactorInterface; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::generateUUID; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::SteeringFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactor; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::PlanningFactor; @@ -106,26 +98,6 @@ class SceneModuleInterface } } - // TODO(satoshi-ota): remove this constructor after all planning factors have been migrated. - SceneModuleInterface( - const std::string & name, rclcpp::Node & node, - std::unordered_map> rtc_interface_ptr_map, - std::unordered_map> - objects_of_interest_marker_interface_ptr_map) - : name_{name}, - logger_{node.get_logger().get_child(name)}, - clock_{node.get_clock()}, - rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), - objects_of_interest_marker_interface_ptr_map_( - std::move(objects_of_interest_marker_interface_ptr_map)), - planning_factor_interface_{std::make_shared(&node, "tmp")}, - time_keeper_(std::make_shared()) - { - for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { - uuid_map_.emplace(module_name, generateUUID()); - } - } - SceneModuleInterface(const SceneModuleInterface &) = delete; SceneModuleInterface(SceneModuleInterface &&) = delete; SceneModuleInterface & operator=(const SceneModuleInterface &) = delete; @@ -210,8 +182,6 @@ class SceneModuleInterface unlockNewModuleLaunch(); unlockOutputPath(); - reset_factor(); - processOnExit(); } @@ -284,16 +254,6 @@ class SceneModuleInterface ModuleStatus getCurrentStatus() const { return current_state_; } - void reset_factor() - { - steering_factor_interface_.reset(); - velocity_factor_interface_.reset(); - } - - auto get_steering_factor() const -> SteeringFactor { return steering_factor_interface_.get(); } - - auto get_velocity_factor() const -> VelocityFactor { return velocity_factor_interface_.get(); } - std::string name() const { return name_; } PoseWithDetailOpt getStopPose() const @@ -584,22 +544,6 @@ class SceneModuleInterface } } - void setVelocityFactor(const PathWithLaneId & path) - { - if (stop_pose_.has_value()) { - velocity_factor_interface_.set( - path.points, getEgoPose(), stop_pose_.value().pose, VelocityFactor::APPROACHING, "stop"); - return; - } - - if (!slow_pose_.has_value()) { - return; - } - - velocity_factor_interface_.set( - path.points, getEgoPose(), slow_pose_.value().pose, VelocityFactor::APPROACHING, "slow down"); - } - void set_longitudinal_planning_factor(const PathWithLaneId & path) { if (stop_pose_.has_value()) { @@ -673,10 +617,6 @@ class SceneModuleInterface mutable std::shared_ptr planning_factor_interface_; - mutable SteeringFactorInterface steering_factor_interface_; - - mutable VelocityFactorInterface velocity_factor_interface_; - mutable PoseWithDetailOpt stop_pose_{std::nullopt}; mutable PoseWithDetailOpt slow_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 74ca572c72909..446829d1d0f9d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -24,8 +24,6 @@ #include #include -#include -#include #include #include @@ -42,8 +40,6 @@ using autoware::motion_utils::createDeadLineVirtualWallMarker; using autoware::motion_utils::createSlowDownVirtualWallMarker; using autoware::motion_utils::createStopVirtualWallMarker; using autoware::universe_utils::toHexString; -using autoware_adapi_v1_msgs::msg::SteeringFactorArray; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; using SceneModuleObserver = std::weak_ptr; @@ -106,46 +102,6 @@ class SceneModuleManagerInterface } } - void publishSteeringFactor() - { - SteeringFactorArray steering_factor_array; - steering_factor_array.header.frame_id = "map"; - steering_factor_array.header.stamp = node_->now(); - - for (const auto & m : observers_) { - if (m.expired()) { - continue; - } - - const auto steering_factor = m.lock()->get_steering_factor(); - if (steering_factor.behavior != PlanningBehavior::UNKNOWN) { - steering_factor_array.factors.emplace_back(steering_factor); - } - } - - pub_steering_factors_->publish(steering_factor_array); - } - - void publishVelocityFactor() - { - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = node_->now(); - - for (const auto & m : observers_) { - if (m.expired()) { - continue; - } - - const auto velocity_factor = m.lock()->get_velocity_factor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - } - - pub_velocity_factors_->publish(velocity_factor_array); - } - void publish_planning_factors() { planning_factor_interface_->publish(); } void publishVirtualWall() const @@ -306,10 +262,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_drivable_lanes_; - rclcpp::Publisher::SharedPtr pub_steering_factors_; - - rclcpp::Publisher::SharedPtr pub_velocity_factors_; - rclcpp::Publisher::SharedPtr pub_processing_time_; std::string name_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp index 3d3a57c8a67b7..11be3b28a7f94 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp @@ -61,10 +61,6 @@ void SceneModuleManagerInterface::initInterface( pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name_, 20); pub_processing_time_ = node->create_publisher( "~/processing_time/" + name_, 20); - pub_steering_factors_ = - node->create_publisher("/planning/steering_factor/" + name_, 1); - pub_velocity_factors_ = - node->create_publisher("/planning/velocity_factors/" + name_, 1); } // planning factor