fix(obstacle_stop_planner): migrate planning factor (#9939)

* fix(obstacle_stop_planner): migrate planning factor Signed-off-by: satoshi-ota <[email protected]> * fix(autoware_default_adapi): add coversion map Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Jan 17, 2025 · 08dd641 · 08dd641
1 parent 67ab350
commit 08dd641
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 31 deletions.
diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp
@@ -45,12 +45,13 @@ namespace autoware::motion_planning
 {
 ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode(
   rclcpp::Node * node, const double base_link2front)
-: node_(node), base_link2front_(base_link2front)
+: node_(node),
+  base_link2front_(base_link2front),
+  planning_factor_interface_{std::make_unique<autoware::motion_utils::PlanningFactorInterface>(
+    node, "obstacle_stop_planner")}
 {
   virtual_wall_pub_ = node_->create_publisher<MarkerArray>("~/virtual_wall", 1);
   debug_viz_pub_ = node_->create_publisher<MarkerArray>("~/debug/marker", 1);
-  velocity_factor_pub_ =
-    node_->create_publisher<VelocityFactorArray>("/planning/velocity_factors/obstacle_stop", 1);
   pub_debug_values_ =
     node_->create_publisher<Float32MultiArrayStamped>("~/obstacle_stop/debug_values", 1);
 }
@@ -189,8 +190,13 @@ void ObstacleStopPlannerDebugNode::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_) {
+    planning_factor_interface_->add(
+      std::numeric_limits<float>::quiet_NaN(), *stop_pose_ptr_,
+      tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{},
+      true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "");
+  }
+  planning_factor_interface_->publish();
 
   // publish debug values
   Float32MultiArrayStamped debug_msg{};
@@ -492,23 +498,4 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker()
   return msg;
 }
 
-VelocityFactorArray ObstacleStopPlannerDebugNode::makeVelocityFactorArray()
-{
-  VelocityFactorArray velocity_factor_array;
-  velocity_factor_array.header.frame_id = "map";
-  velocity_factor_array.header.stamp = node_->now();
-
-  if (stop_pose_ptr_) {
-    using distance_type = VelocityFactor::_distance_type;
-    VelocityFactor velocity_factor;
-    velocity_factor.behavior = PlanningBehavior::ROUTE_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;
-}
-
 }  // namespace autoware::motion_planning
diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp
@@ -14,10 +14,9 @@
 #ifndef DEBUG_MARKER_HPP_
 #define DEBUG_MARKER_HPP_
 
+#include <autoware/motion_utils/factor/planning_factor_interface.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/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <tier4_debug_msgs/msg/float32_multi_array_stamped.hpp>
@@ -42,9 +41,6 @@ using std_msgs::msg::Header;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
-using autoware_adapi_v1_msgs::msg::PlanningBehavior;
-using autoware_adapi_v1_msgs::msg::VelocityFactor;
-using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
 using tier4_debug_msgs::msg::Float32MultiArrayStamped;
 
 enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown, Obstacle };
@@ -109,7 +105,6 @@ class ObstacleStopPlannerDebugNode
   bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type);
   MarkerArray makeVirtualWallMarker();
   MarkerArray makeVisualizationMarker();
-  VelocityFactorArray makeVelocityFactorArray();
 
   void setDebugValues(const DebugValues::TYPE type, const double val)
   {
@@ -120,11 +115,12 @@ class ObstacleStopPlannerDebugNode
 private:
   rclcpp::Publisher<MarkerArray>::SharedPtr virtual_wall_pub_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_viz_pub_;
-  rclcpp::Publisher<VelocityFactorArray>::SharedPtr velocity_factor_pub_;
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr pub_debug_values_;
   rclcpp::Node * node_;
   double base_link2front_;
 
+  std::unique_ptr<autoware::motion_utils::PlanningFactorInterface> planning_factor_interface_;
+
   std::shared_ptr<Pose> stop_pose_ptr_;
   std::shared_ptr<Pose> target_stop_pose_ptr_;
   std::shared_ptr<Pose> slow_down_start_pose_ptr_;

diff --git a/system/autoware_default_adapi/src/planning.cpp b/system/autoware_default_adapi/src/planning.cpp
@@ -44,6 +44,7 @@ const std::map<std::string, std::string> conversion_map = {
   {"no_stopping_area", PlanningBehavior::NO_STOPPING_AREA},
   {"blind_spot", PlanningBehavior::REAR_CHECK},
   {"obstacle_cruise_planner", PlanningBehavior::ROUTE_OBSTACLE},
+  {"obstacle_stop_planner", PlanningBehavior::ROUTE_OBSTACLE},
   {"motion_velocity_planner", PlanningBehavior::ROUTE_OBSTACLE},
   {"walkway", PlanningBehavior::SIDEWALK},
   {"start_planner", PlanningBehavior::START_PLANNER},
@@ -215,6 +216,7 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning
     "/planning/planning_factors/merge_from_private",
     "/planning/planning_factors/no_stopping_area",
     "/planning/planning_factors/obstacle_cruise_planner",
+    "/planning/planning_factors/obstacle_stop_planner",
     "/planning/planning_factors/occlusion_spot",
     "/planning/planning_factors/run_out",
     "/planning/planning_factors/stop_line",