diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index bbd1cac04ed89..7cce54bd62fc6 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -67,7 +67,9 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const double & surround_check_back_distance, const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) -: vehicle_info_(vehicle_info), +: planning_factor_interface_{std::make_unique( + &node, "surround_obstacle_checker")}, + vehicle_info_(vehicle_info), object_label_(object_label), surround_check_front_distance_(surround_check_front_distance), surround_check_side_distance_(surround_check_side_distance), @@ -145,6 +147,12 @@ void SurroundObstacleCheckerDebugNode::publish() /* publish stop reason for autoware api */ const auto velocity_factor_msg = makeVelocityFactorArray(); velocity_factor_pub_->publish(velocity_factor_msg); + if (stop_pose_ptr_ != nullptr) { + planning_factor_interface_->add( + 0.0, *stop_pose_ptr_, tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); + } + planning_factor_interface_->publish(); /* reset variables */ stop_pose_ptr_ = nullptr; diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index b2c350c1b4698..36dbfd32b1a03 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -15,6 +15,7 @@ #ifndef DEBUG_MARKER_HPP_ #define DEBUG_MARKER_HPP_ +#include #include #include @@ -22,6 +23,7 @@ #include #include #include +#include #include #include @@ -38,6 +40,9 @@ using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using geometry_msgs::msg::PolygonStamped; +using tier4_planning_msgs::msg::ControlPoint; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -71,6 +76,8 @@ class SurroundObstacleCheckerDebugNode rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; + std::unique_ptr planning_factor_interface_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::string object_label_; double surround_check_front_distance_;