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 041b7ad20a107..372893d1b1a45 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 @@ -184,6 +184,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da m->publishRTCStatus(); m->publishSteeringFactor(); m->publishVelocityFactor(); + m->publish_planning_factors(); }); generateCombinedDrivableArea(result_output.valid_output, data); 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 391ac92411220..4e9f433f11469 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 @@ -21,6 +21,7 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include +#include #include #include #include @@ -54,6 +55,7 @@ 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; @@ -66,6 +68,8 @@ 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; +using tier4_planning_msgs::msg::SafetyFactorArray; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; @@ -82,6 +86,27 @@ enum class ModuleStatus { class SceneModuleInterface { public: + 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, + const std::shared_ptr planning_factor_interface) + : 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_{planning_factor_interface}, + time_keeper_(std::make_shared()) + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + uuid_map_.emplace(module_name, generateUUID()); + } + } + + // 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, @@ -93,6 +118,7 @@ class SceneModuleInterface 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_) { @@ -574,6 +600,24 @@ class SceneModuleInterface path.points, getEgoPose(), slow_pose_.value().pose, VelocityFactor::APPROACHING, "slow down"); } + void set_longitudinal_planning_factor(const PathWithLaneId & path) + { + if (stop_pose_.has_value()) { + planning_factor_interface_->add( + path.points, getEgoPose(), stop_pose_.value().pose, PlanningFactor::STOP, + SafetyFactorArray{}); + return; + } + + if (!slow_pose_.has_value()) { + return; + } + + planning_factor_interface_->add( + path.points, getEgoPose(), slow_pose_.value().pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}); + } + void setDrivableLanes(const std::vector & drivable_lanes); BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } @@ -627,6 +671,8 @@ class SceneModuleInterface std::unordered_map> objects_of_interest_marker_interface_ptr_map_; + mutable std::shared_ptr planning_factor_interface_; + mutable SteeringFactorInterface steering_factor_interface_; mutable VelocityFactorInterface velocity_factor_interface_; 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 73b133952935b..74ca572c72909 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 @@ -146,6 +146,8 @@ class SceneModuleManagerInterface pub_velocity_factors_->publish(velocity_factor_array); } + void publish_planning_factors() { planning_factor_interface_->publish(); } + void publishVirtualWall() const { using autoware::universe_utils::appendMarkerArray; @@ -318,6 +320,8 @@ class SceneModuleManagerInterface std::unique_ptr idle_module_ptr_; + std::shared_ptr planning_factor_interface_; + std::unordered_map> rtc_interface_ptr_map_; std::unordered_map> 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 b91db3a740cfe..3d3a57c8a67b7 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 @@ -67,6 +67,11 @@ void SceneModuleManagerInterface::initInterface( node->create_publisher("/planning/velocity_factors/" + name_, 1); } + // planning factor + { + planning_factor_interface_ = std::make_shared(node, name_); + } + // misc { node_ = node;