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);