tmp

Signed-off-by: satoshi-ota <[email protected]>

planning/autoware_surround_obstacle_checker/package.xml

@@ -14,6 +14,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
   <buildtool_depend>eigen3_cmake_module</buildtool_depend>
 
+  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_motion_utils</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>

planning/autoware_surround_obstacle_checker/src/debug_marker.cpp

@@ -79,6 +79,8 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode(
   clock_(clock)
 {
   debug_viz_pub_ = node.create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/marker", 1);
+  velocity_factor_pub_ =
+    node.create_publisher<VelocityFactorArray>("/planning/velocity_factors/surround_obstacle", 1);
   vehicle_footprint_pub_ = node.create_publisher<PolygonStamped>("~/debug/footprint", 1);
   vehicle_footprint_offset_pub_ =
     node.create_publisher<PolygonStamped>("~/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<distance_type>::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)
 {

planning/autoware_surround_obstacle_checker/src/debug_marker.hpp

@@ -19,6 +19,8 @@
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
+#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
 #include <geometry_msgs/msg/polygon_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <tier4_planning_msgs/msg/planning_factor_array.hpp>
@@ -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<MarkerArray>::SharedPtr debug_viz_pub_;
+  rclcpp::Publisher<VelocityFactorArray>::SharedPtr velocity_factor_pub_;
 
   rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_pub_;
   rclcpp::Publisher<PolygonStamped>::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);