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 29c2d60a30d9..bd5b39112746 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 @@ -178,6 +178,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da m->updateObserver(); m->publishRTCStatus(); m->publishSteeringFactor(); + m->publishVelocityFactor(); }); 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 60a5faf6cd92..5da5c9ccc6c1 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,6 +22,7 @@ #include #include +#include #include #include #include @@ -57,6 +58,7 @@ namespace autoware::behavior_path_planner { 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; @@ -64,6 +66,7 @@ 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::StopFactor; @@ -265,10 +268,16 @@ class SceneModuleInterface StopReason getStopReason() const { return stop_reason_; } - void reset_factor() { steering_factor_interface_.reset(); } + 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_; } std::optional getStopPose() const @@ -555,6 +564,22 @@ class SceneModuleInterface } } + void setVelocityFactor(const PathWithLaneId & path) + { + if (stop_pose_.has_value()) { + velocity_factor_interface_.set( + path.points, getEgoPose(), stop_pose_.value(), VelocityFactor::APPROACHING, "stop"); + return; + } + + if (!slow_pose_.has_value()) { + return; + } + + velocity_factor_interface_.set( + path.points, getEgoPose(), slow_pose_.value(), VelocityFactor::APPROACHING, "slow down"); + } + void setStopReason(const std::string & stop_reason, const PathWithLaneId & path) { stop_reason_.reason = stop_reason; @@ -627,6 +652,8 @@ class SceneModuleInterface mutable SteeringFactorInterface steering_factor_interface_; + mutable VelocityFactorInterface velocity_factor_interface_; + mutable std::optional stop_pose_{std::nullopt}; mutable std::optional 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 65df5e15dad4..e199309dea8b 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,6 +24,7 @@ #include #include +#include #include #include @@ -42,6 +43,7 @@ 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; @@ -124,6 +126,26 @@ class SceneModuleManagerInterface 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 publishVirtualWall() const { using autoware::universe_utils::appendMarkerArray; @@ -286,6 +308,8 @@ class SceneModuleManagerInterface 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 35f29921b97b..7b362d9a8baf 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 @@ -59,6 +59,8 @@ void SceneModuleManagerInterface::initInterface( "~/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); } // misc diff --git a/system/autoware_default_adapi/src/planning.cpp b/system/autoware_default_adapi/src/planning.cpp index 4ff0edfd2b7d..d89ef6c22166 100644 --- a/system/autoware_default_adapi/src/planning.cpp +++ b/system/autoware_default_adapi/src/planning.cpp @@ -87,7 +87,14 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning "/planning/velocity_factors/traffic_light", "/planning/velocity_factors/virtual_traffic_light", "/planning/velocity_factors/walkway", - "/planning/velocity_factors/motion_velocity_planner"}; + "/planning/velocity_factors/motion_velocity_planner", + "/planning/velocity_factors/static_obstacle_avoidance", + "/planning/velocity_factors/dynamic_obstacle_avoidance", + "/planning/velocity_factors/avoidance_by_lane_change", + "/planning/velocity_factors/lane_change_left", + "/planning/velocity_factors/lane_change_right", + "/planning/velocity_factors/start_planner", + "/planning/velocity_factors/goal_planner"}; std::vector steering_factor_topics = { "/planning/steering_factor/static_obstacle_avoidance",