diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 12cb59ac46195..bb2107e2a3117 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -45,6 +45,9 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo clock_ = node.get_clock(); velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + planning_factor_interface_ = std::make_unique( + &node, "dynamic_obstacle_stop"); + debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); virtual_wall_publisher_ = @@ -162,6 +165,9 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( const auto stop_pose_reached = planner_data->current_odometry.twist.twist.linear.x < 1e-3 && autoware::universe_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; + planning_factor_interface_->add( + ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP, + SafetyFactorArray{}); velocity_factor_interface_.set( ego_trajectory_points, ego_data.pose, *stop_pose, stop_pose_reached ? autoware::motion_utils::VelocityFactor::STOPPED diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp index 1843d26a22a29..82a6d790bebe7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp @@ -33,6 +33,7 @@ class DynamicObstacleStopModule : public PluginModuleInterface { public: void init(rclcpp::Node & node, const std::string & module_name) override; + void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( const std::vector & ego_trajectory_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp index 9bd662af697ea..4c923afe03f65 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp @@ -18,6 +18,7 @@ #include "planner_data.hpp" #include "velocity_planning_result.hpp" +#include #include #include #include @@ -32,6 +33,9 @@ namespace autoware::motion_velocity_planner { +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; + class PluginModuleInterface { public: @@ -42,6 +46,7 @@ class PluginModuleInterface const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; + virtual void publish_planning_factor() {} autoware::motion_utils::VelocityFactorInterface velocity_factor_interface_; rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; @@ -49,6 +54,9 @@ class PluginModuleInterface std::shared_ptr processing_diag_publisher_; rclcpp::Publisher::SharedPtr processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; + +protected: + std::unique_ptr planning_factor_interface_; }; } // namespace autoware::motion_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index b08798cbef772..5bdf386479283 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -102,6 +102,8 @@ std::vector MotionVelocityPlannerManager::plan_velocitie VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); results.push_back(res); + plugin->publish_planning_factor(); + if (res.stop_points.size() > 0) { const auto stop_decision_metric = make_decision_metric(plugin->get_module_name(), "stop"); metrics_.push_back(stop_decision_metric);