diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 8bbc0b6bf4e02..4a6f946b8eb90 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -14,6 +14,7 @@ autoware_cmake eigen3_cmake_module + autoware_adapi_v1_msgs autoware_motion_utils autoware_perception_msgs autoware_planning_msgs diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index 2acf6ba1c92f5..7cce54bd62fc6 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -79,6 +79,8 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( clock_(clock) { debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); + velocity_factor_pub_ = + node.create_publisher("/planning/velocity_factors/surround_obstacle", 1); vehicle_footprint_pub_ = node.create_publisher("~/debug/footprint", 1); vehicle_footprint_offset_pub_ = node.create_publisher("~/debug/footprint_offset", 1); @@ -143,6 +145,8 @@ void SurroundObstacleCheckerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* 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, @@ -174,6 +178,25 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() return msg; } +VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() +{ + VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); + + if (stop_pose_ptr_) { + using distance_type = VelocityFactor::_distance_type; + VelocityFactor velocity_factor; + velocity_factor.behavior = PlanningBehavior::SURROUNDING_OBSTACLE; + velocity_factor.pose = *stop_pose_ptr_; + velocity_factor.distance = std::numeric_limits::quiet_NaN(); + velocity_factor.status = VelocityFactor::UNKNOWN; + velocity_factor.detail = std::string(); + velocity_factor_array.factors.push_back(velocity_factor); + } + return velocity_factor_array; +} + PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( const Polygon2d & boost_polygon, const double & z) { diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index c49e277f2dc6c..36dbfd32b1a03 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include #include #include @@ -34,6 +36,9 @@ namespace autoware::surround_obstacle_checker { using autoware::vehicle_info_utils::VehicleInfo; +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; @@ -65,6 +70,7 @@ class SurroundObstacleCheckerDebugNode private: rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; @@ -81,6 +87,7 @@ class SurroundObstacleCheckerDebugNode geometry_msgs::msg::Pose self_pose_; MarkerArray makeVisualizationMarker(); + VelocityFactorArray makeVelocityFactorArray(); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z);