From 39677a675e0ea362e27385bbe20a7bd69cfcf94a Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Thu, 26 Dec 2024 08:46:57 +0900
Subject: [PATCH] fix(surround_obstacle_checker): remove velocity factor

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../package.xml                               |  1 -
 .../src/debug_marker.cpp                      | 23 -------------------
 .../src/debug_marker.hpp                      |  7 ------
 3 files changed, 31 deletions(-)

diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml
index 4a6f946b8eb90..8bbc0b6bf4e02 100644
--- a/planning/autoware_surround_obstacle_checker/package.xml
+++ b/planning/autoware_surround_obstacle_checker/package.xml
@@ -14,7 +14,6 @@
   <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>
diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp
index 7cce54bd62fc6..2acf6ba1c92f5 100644
--- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp
+++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp
@@ -79,8 +79,6 @@ 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);
@@ -145,8 +143,6 @@ 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,
@@ -178,25 +174,6 @@ 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)
 {
diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp
index 36dbfd32b1a03..c49e277f2dc6c 100644
--- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp
+++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp
@@ -19,8 +19,6 @@
 #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>
@@ -36,9 +34,6 @@ 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;
@@ -70,7 +65,6 @@ 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_;
@@ -87,7 +81,6 @@ class SurroundObstacleCheckerDebugNode
   geometry_msgs::msg::Pose self_pose_;
 
   MarkerArray makeVisualizationMarker();
-  VelocityFactorArray makeVelocityFactorArray();
 
   PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z);