feat(bpp): add velocity interface

planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp

@@ -178,6 +178,7 @@
     m->updateObserver();
     m->publishRTCStatus();
     m->publishSteeringFactor();
+    m->publishVelocityFactor();
   });
 
   generateCombinedDrivableArea(result_output.valid_output, data);

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp

@@ -22,6 +22,7 @@
 
 #include <autoware/behavior_path_planner_common/turn_signal_decider.hpp>
 #include <autoware/motion_utils/factor/steering_factor_interface.hpp>
+#include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
 #include <autoware/motion_utils/marker/marker_helper.hpp>
 #include <autoware/motion_utils/trajectory/path_with_lane_id.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
@@ -57,13 +58,15 @@
 namespace autoware::behavior_path_planner
 {
 using autoware::motion_utils::SteeringFactorInterface;
+using autoware::motion_utils::VelocityFactorInterface;
 using autoware::objects_of_interest_marker_interface::ColorName;
 using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface;
 using autoware::rtc_interface::RTCInterface;
 using autoware::universe_utils::calcOffsetPose;
 using autoware::universe_utils::generateUUID;
 using autoware_adapi_v1_msgs::msg::PlanningBehavior;
 using autoware_adapi_v1_msgs::msg::SteeringFactor;
+using autoware_adapi_v1_msgs::msg::VelocityFactor;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::PathWithLaneId;
 using tier4_planning_msgs::msg::StopFactor;
@@ -265,10 +268,16 @@
 
   StopReason getStopReason() const { return stop_reason_; }
 
-  void reset_factor() { steering_factor_interface_.reset(); }
+  void reset_factor()
+  {
+    steering_factor_interface_.reset();
+    velocity_factor_interface_.reset();
+  }
 
   auto get_steering_factor() const -> SteeringFactor { return steering_factor_interface_.get(); }
 
+  auto get_velocity_factor() const -> VelocityFactor { return velocity_factor_interface_.get(); }
+
   std::string name() const { return name_; }
 
   std::optional<Pose> getStopPose() const
@@ -555,6 +564,22 @@
     }
   }
 
+  void setVelocityFactor(const PathWithLaneId & path)
+  {
+    if (stop_pose_.has_value()) {
+      velocity_factor_interface_.set(
+        path.points, getEgoPose(), stop_pose_.value(), VelocityFactor::APPROACHING, "stop");
+      return;
+    }
+
+    if (!slow_pose_.has_value()) {
+      return;
+    }
+
+    velocity_factor_interface_.set(
+      path.points, getEgoPose(), slow_pose_.value(), VelocityFactor::APPROACHING, "slow down");
+  }
+
   void setStopReason(const std::string & stop_reason, const PathWithLaneId & path)
   {
     stop_reason_.reason = stop_reason;
@@ -627,6 +652,8 @@
 
   mutable SteeringFactorInterface steering_factor_interface_;
 
+  mutable VelocityFactorInterface velocity_factor_interface_;
+
   mutable std::optional<Pose> stop_pose_{std::nullopt};
 
   mutable std::optional<Pose> slow_pose_{std::nullopt};

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp

@@ -24,6 +24,7 @@
 #include <rclcpp/publisher.hpp>
 
 #include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp>
+#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <cstddef>
@@ -42,6 +43,7 @@
 using autoware::motion_utils::createStopVirtualWallMarker;
 using autoware::universe_utils::toHexString;
 using autoware_adapi_v1_msgs::msg::SteeringFactorArray;
+using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
 using unique_identifier_msgs::msg::UUID;
 using SceneModulePtr = std::shared_ptr<SceneModuleInterface>;
 using SceneModuleObserver = std::weak_ptr<SceneModuleInterface>;
@@ -124,6 +126,26 @@
     pub_steering_factors_->publish(steering_factor_array);
   }
 
+  void publishVelocityFactor()
+  {
+    VelocityFactorArray velocity_factor_array;
+    velocity_factor_array.header.frame_id = "map";
+    velocity_factor_array.header.stamp = node_->now();
+
+    for (const auto & m : observers_) {
+      if (m.expired()) {
+        continue;
+      }
+
+      const auto velocity_factor = m.lock()->get_velocity_factor();
+      if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) {
+        velocity_factor_array.factors.emplace_back(velocity_factor);
+      }
+    }
+
+    pub_velocity_factors_->publish(velocity_factor_array);
+  }
+
   void publishVirtualWall() const
   {
     using autoware::universe_utils::appendMarkerArray;
@@ -286,6 +308,8 @@
 
   rclcpp::Publisher<SteeringFactorArray>::SharedPtr pub_steering_factors_;
 
+  rclcpp::Publisher<VelocityFactorArray>::SharedPtr pub_velocity_factors_;
+
   rclcpp::Publisher<universe_utils::ProcessingTimeDetail>::SharedPtr pub_processing_time_;
 
   std::string name_;

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp

@@ -59,6 +59,8 @@ void SceneModuleManagerInterface::initInterface(
       "~/processing_time/" + name_, 20);
     pub_steering_factors_ =
       node->create_publisher<SteeringFactorArray>("/planning/steering_factor/" + name_, 1);
+    pub_velocity_factors_ =
+      node->create_publisher<VelocityFactorArray>("/planning/velocity_factors/" + name_, 1);
   }
 
   // misc

system/autoware_default_adapi/src/planning.cpp

@@ -87,7 +87,14 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning
     "/planning/velocity_factors/traffic_light",
     "/planning/velocity_factors/virtual_traffic_light",
     "/planning/velocity_factors/walkway",
-    "/planning/velocity_factors/motion_velocity_planner"};
+    "/planning/velocity_factors/motion_velocity_planner",
+    "/planning/velocity_factors/static_obstacle_avoidance",
+    "/planning/velocity_factors/dynamic_obstacle_avoidance",
+    "/planning/velocity_factors/avoidance_by_lane_change",
+    "/planning/velocity_factors/lane_change_left",
+    "/planning/velocity_factors/lane_change_right",
+    "/planning/velocity_factors/start_planner",
+    "/planning/velocity_factors/goal_planner"};
 
   std::vector<std::string> steering_factor_topics = {
     "/planning/steering_factor/static_obstacle_avoidance",