refactor(bpp): rework steering factor interface

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp

@@ -31,15 +31,13 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface(
   const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
   const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-    objects_of_interest_marker_interface_ptr_map,
-  std::shared_ptr<SteeringFactorInterface> & steering_factor_interface_ptr)
+    objects_of_interest_marker_interface_ptr_map)
 : LaneChangeInterface{
     name,
     node,
     parameters,
     rtc_interface_ptr_map,
     objects_of_interest_marker_interface_ptr_map,
-    steering_factor_interface_ptr,
     std::make_unique<AvoidanceByLaneChange>(parameters, avoidance_by_lane_change_parameters)}
 {
 }

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp

@@ -40,8 +40,7 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface
     const std::shared_ptr<AvoidanceByLCParameters> & avoidance_by_lane_change_parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-      objects_of_interest_marker_interface_ptr_map,
-    std::shared_ptr<SteeringFactorInterface> & steering_factor_interface_ptr);
+      objects_of_interest_marker_interface_ptr_map);
 
   bool isExecutionRequested() const override;
 

planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -190,7 +190,7 @@ SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<AvoidanceByLaneChangeInterface>(
     name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_);
+    objects_of_interest_marker_interface_ptr_map_);
 }
 
 }  // namespace autoware::behavior_path_planner

planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp

@@ -42,7 +42,7 @@
   {
     return std::make_unique<DynamicObstacleAvoidanceModule>(
       name_, *node_, parameters_, rtc_interface_ptr_map_,
-      objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_);
+      objects_of_interest_marker_interface_ptr_map_);
   }
 
   void updateModuleParams(const std::vector<rclcpp::Parameter> & parameters) override;

planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp

@@ -350,8 +350,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface
     std::shared_ptr<DynamicAvoidanceParameters> parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-      objects_of_interest_marker_interface_ptr_map,
-    std::shared_ptr<SteeringFactorInterface> & steering_factor_interface_ptr);
+      objects_of_interest_marker_interface_ptr_map);
 
   void updateModuleParams(const std::any & parameters) override
   {

planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp

@@ -329,9 +329,8 @@
   std::shared_ptr<DynamicAvoidanceParameters> parameters,
   const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-    objects_of_interest_marker_interface_ptr_map,
-  std::shared_ptr<SteeringFactorInterface> & steering_factor_interface_ptr)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr},  // NOLINT
+    objects_of_interest_marker_interface_ptr_map)
+: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{std::move(parameters)},
   target_objects_manager_{TargetObjectsManager(
     parameters_->successive_num_to_entry_dynamic_avoidance_condition,

planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp

@@ -26,7 +26,7 @@ ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<LaneChangeInterface>(
     name_, *node_, parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_,
+    objects_of_interest_marker_interface_ptr_map_,
     std::make_unique<ExternalRequestLaneChange>(parameters_, direction_));
 }
 
@@ -35,7 +35,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance()
 {
   return std::make_unique<LaneChangeInterface>(
     name_, *node_, parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_,
+    objects_of_interest_marker_interface_ptr_map_,
     std::make_unique<ExternalRequestLaneChange>(parameters_, direction_));
 }
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

@@ -142,8 +142,7 @@ class GoalPlannerModule : public SceneModuleInterface
     const std::shared_ptr<GoalPlannerParameters> & parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-      objects_of_interest_marker_interface_ptr_map,
-    std::shared_ptr<SteeringFactorInterface> & steering_factor_interface_ptr);
+      objects_of_interest_marker_interface_ptr_map);
 
   ~GoalPlannerModule()
   {

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -63,9 +63,8 @@
   const std::shared_ptr<GoalPlannerParameters> & parameters,
   const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
-    objects_of_interest_marker_interface_ptr_map,
-  std::shared_ptr<SteeringFactorInterface> & steering_factor_interface_ptr)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr},  // NOLINT
+    objects_of_interest_marker_interface_ptr_map)
+: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{parameters},
   vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()},
   thread_safe_data_{mutex_, clock_},
@@ -135,6 +134,8 @@
     initializeSafetyCheckParameters();
   }
 
+  steering_factor_interface_.init(PlanningBehavior::GOAL_PLANNER);
+
   /**
    * NOTE: Add `universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);` to functions called
    * from the main thread only.
@@ -1262,8 +1263,7 @@
     return SteeringFactor::STRAIGHT;
   });
 
-  steering_factor_interface_ptr_->updateSteeringFactor(
-    pose, distance, PlanningBehavior::GOAL_PLANNER, steering_factor_direction, type, "");
+  steering_factor_interface_.set(pose, distance, steering_factor_direction, type, "");
 }
 
 void GoalPlannerModule::decideVelocity()

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp

@@ -31,7 +31,7 @@
 {
   return std::make_unique<GoalPlannerModule>(
     name_, *node_, parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_);
+    objects_of_interest_marker_interface_ptr_map_);
 }
 
 GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters(

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp

@@ -52,7 +52,6 @@ class LaneChangeInterface : public SceneModuleInterface
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
       objects_of_interest_marker_interface_ptr_map,
-    std::shared_ptr<SteeringFactorInterface> & steering_factor_interface_ptr,
     std::unique_ptr<LaneChangeBase> && module_type);
 
   LaneChangeInterface(const LaneChangeInterface &) = delete;

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

@@ -38,15 +38,15 @@ LaneChangeInterface::LaneChangeInterface(
   const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map,
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
     objects_of_interest_marker_interface_ptr_map,
-  std::shared_ptr<SteeringFactorInterface> & steering_factor_interface_ptr,
   std::unique_ptr<LaneChangeBase> && module_type)
-: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, steering_factor_interface_ptr},  // NOLINT
+: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map},  // NOLINT
   parameters_{std::move(parameters)},
   module_type_{std::move(module_type)},
   prev_approved_path_{std::make_unique<PathWithLaneId>()}
 {
   module_type_->setTimeKeeper(getTimeKeeper());
   logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr());
+  steering_factor_interface_.init(PlanningBehavior::LANE_CHANGE);
 }
 
 void LaneChangeInterface::processOnExit()
@@ -422,10 +422,9 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o
   const auto finish_distance = autoware::motion_utils::calcSignedArcLength(
     output.path.points, current_position, status.lane_change_path.info.shift_line.end.position);
 
-  steering_factor_interface_ptr_->updateSteeringFactor(
+  steering_factor_interface_.set(
     {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end},
-    {start_distance, finish_distance}, PlanningBehavior::LANE_CHANGE, steering_factor_direction,
-    SteeringFactor::TURNING, "");
+    {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, "");
 }
 
 void LaneChangeInterface::updateSteeringFactorPtr(
@@ -438,9 +437,9 @@ void LaneChangeInterface::updateSteeringFactorPtr(
     return SteeringFactor::RIGHT;
   });
 
-  steering_factor_interface_ptr_->updateSteeringFactor(
+  steering_factor_interface_.set(
     {selected_path.info.shift_line.start, selected_path.info.shift_line.end},
     {output.start_distance_to_path_change, output.finish_distance_to_path_change},
-    PlanningBehavior::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, "");
+    steering_factor_direction, SteeringFactor::APPROACHING, "");
 }
 }  // namespace autoware::behavior_path_planner

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp

@@ -310,7 +310,7 @@ std::unique_ptr<SceneModuleInterface> LaneChangeModuleManager::createNewSceneMod
 {
   return std::make_unique<LaneChangeInterface>(
     name_, *node_, parameters_, rtc_interface_ptr_map_,
-    objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_,
+    objects_of_interest_marker_interface_ptr_map_,
     std::make_unique<NormalLaneChange>(parameters_, LaneChangeModuleType::NORMAL, direction_));
 }
 

planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp

@@ -124,6 +124,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<PoseWithUuidStamped>::SharedPtr modified_goal_publisher_;
   rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reason_publisher_;
   rclcpp::Publisher<RerouteAvailability>::SharedPtr reroute_availability_publisher_;
+  rclcpp::Publisher<SteeringFactorArray>::SharedPtr pub_steering_factors_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_candidate_publishers_;
@@ -138,7 +139,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
   std::shared_ptr<PlannerManager> planner_manager_;
 
-  std::unique_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;
+  SteeringFactorInterface steering_factor_interface_;
 
   std::mutex mutex_pd_;       // mutex for planner_data_
   std::mutex mutex_manager_;  // mutex for bt_manager_ or planner_manager_

planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021-2023 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -52,69 +52,71 @@
   modified_goal_publisher_ =
     create_publisher<PoseWithUuidStamped>("~/output/modified_goal", durable_qos);
   stop_reason_publisher_ = create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
+  pub_steering_factors_ =
+    create_publisher<SteeringFactorArray>("/planning/steering_factor/intersection", 1);
   reroute_availability_publisher_ =
     create_publisher<RerouteAvailability>("~/output/is_reroute_available", 1);
   debug_avoidance_msg_array_publisher_ =
     create_publisher<AvoidanceDebugMsgArray>("~/debug/avoidance_debug_message_array", 1);
 
   debug_turn_signal_info_publisher_ = create_publisher<MarkerArray>("~/debug/turn_signal_info", 1);
 
   bound_publisher_ = create_publisher<MarkerArray>("~/debug/bound", 1);
 
   {
     const std::string path_candidate_name_space = "/planning/path_candidate/";
     const std::string path_reference_name_space = "/planning/path_reference/";
 
     const std::lock_guard<std::mutex> lock(mutex_manager_);  // for planner_manager_
 
     const auto slots = declare_parameter<std::vector<std::string>>("slots");
     /* cppcheck-suppress syntaxError */
     std::vector<std::vector<std::string>> slot_configuration{slots.size()};
     for (size_t i = 0; i < slots.size(); ++i) {
       const auto & slot = slots.at(i);
       const auto modules = declare_parameter<std::vector<std::string>>(slot);
       for (const auto & module_name : modules) {
         slot_configuration.at(i).push_back(module_name);
       }
     }
 
     planner_manager_ = std::make_shared<PlannerManager>(*this);
 
     for (const auto & name : declare_parameter<std::vector<std::string>>("launch_modules")) {
       // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter.
       if (name == "") {
         break;
       }
       planner_manager_->launchScenePlugin(*this, name);
     }
 
     // NOTE: this needs to be after launchScenePlugin()
     planner_manager_->configureModuleSlot(slot_configuration);
 
     for (const auto & manager : planner_manager_->getSceneModuleManagers()) {
       path_candidate_publishers_.emplace(
         manager->name(), create_publisher<Path>(path_candidate_name_space + manager->name(), 1));
       path_reference_publishers_.emplace(
         manager->name(), create_publisher<Path>(path_reference_name_space + manager->name(), 1));
     }
   }
 
   m_set_param_res = this->add_on_set_parameters_callback(
     std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1));
 
   // turn signal decider
   {
     const double turn_signal_intersection_search_distance =
       planner_data_->parameters.turn_signal_intersection_search_distance;
     const double turn_signal_intersection_angle_threshold_deg =
       planner_data_->parameters.turn_signal_intersection_angle_threshold_deg;
     const double turn_signal_search_time = planner_data_->parameters.turn_signal_search_time;
     planner_data_->turn_signal_decider.setParameters(
       planner_data_->parameters.base_link2front, turn_signal_intersection_search_distance,
       turn_signal_search_time, turn_signal_intersection_angle_threshold_deg);
   }
 
-  steering_factor_interface_ptr_ = std::make_unique<SteeringFactorInterface>(this, "intersection");
+  steering_factor_interface_.init(PlanningBehavior::INTERSECTION);
 
   // Start timer
   {
@@ -473,13 +475,23 @@
     const auto [intersection_pose, intersection_distance] =
       planner_data->turn_signal_decider.getIntersectionPoseAndDistance();
 
-    steering_factor_interface_ptr_->updateSteeringFactor(
+    steering_factor_interface_.set(
       {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance},
-      PlanningBehavior::INTERSECTION, steering_factor_direction, SteeringFactor::TURNING, "");
+      steering_factor_direction, SteeringFactor::TURNING, "");
   } else {
-    steering_factor_interface_ptr_->clearSteeringFactors();
+    steering_factor_interface_.reset();
   }
-  steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now());
+
+  autoware_adapi_v1_msgs::msg::SteeringFactorArray steering_factor_array;
+  steering_factor_array.header.frame_id = "map";
+  steering_factor_array.header.stamp = this->now();
+
+  const auto steering_factor = steering_factor_interface_.get();
+  if (steering_factor.behavior != PlanningBehavior::UNKNOWN) {
+    steering_factor_array.factors.emplace_back(steering_factor);
+  }
+
+  pub_steering_factors_->publish(steering_factor_array);
 }
 
 void BehaviorPathPlannerNode::publish_reroute_availability() const

planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp

@@ -177,6 +177,7 @@
   std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) {
     m->updateObserver();
     m->publishRTCStatus();
+    m->publishSteeringFactor();
   });
 
   generateCombinedDrivableArea(result_output.valid_output, data);
@@ -750,8 +751,6 @@
 
   module_ptr->updateCurrentState();
 
-  module_ptr->publishSteeringFactor();
-
   module_ptr->publishObjectsOfInterestMarker();
 
   processing_time_.at(module_ptr->name()) += stop_watch.toc(module_ptr->name(), true);

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

@@ -74,6 +74,7 @@
 using unique_identifier_msgs::msg::UUID;
 using visualization_msgs::msg::MarkerArray;
 using PlanResult = PathWithLaneId::SharedPtr;
+using autoware_adapi_v1_msgs::msg::PlanningBehavior;
 
 enum class ModuleStatus {
   IDLE = 0,
@@ -90,15 +91,13 @@
     const std::string & name, rclcpp::Node & node,
     std::unordered_map<std::string, std::shared_ptr<RTCInterface>> rtc_interface_ptr_map,
     std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>>
-      objects_of_interest_marker_interface_ptr_map,
-    std::shared_ptr<SteeringFactorInterface> & steering_factor_interface_ptr)
+      objects_of_interest_marker_interface_ptr_map)
   : name_{name},
     logger_{node.get_logger().get_child(name)},
     clock_{node.get_clock()},
     rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)),
     objects_of_interest_marker_interface_ptr_map_(
       std::move(objects_of_interest_marker_interface_ptr_map)),
-    steering_factor_interface_ptr_{steering_factor_interface_ptr},
     time_keeper_(std::make_shared<universe_utils::TimeKeeper>())
   {
     for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
@@ -189,7 +188,8 @@
     clearWaitingApproval();
     unlockNewModuleLaunch();
     unlockOutputPath();
-    steering_factor_interface_ptr_->clearSteeringFactors();
+
+    reset_factor();
 
     stop_reason_ = StopReason();
 
@@ -205,14 +205,6 @@
     }
   }
 
-  void publishSteeringFactor()
-  {
-    if (!steering_factor_interface_ptr_) {
-      return;
-    }
-    steering_factor_interface_ptr_->publishSteeringFactor(clock_->now());
-  }
-
   void lockRTCCommand()
   {
     for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
@@ -275,6 +267,10 @@
 
   StopReason getStopReason() const { return stop_reason_; }
 
+  void reset_factor() { steering_factor_interface_.reset(); }
+
+  auto get_steering_factor() const -> SteeringFactor { return steering_factor_interface_.get(); }
+
   std::string name() const { return name_; }
 
   std::optional<Pose> getStopPose() const
@@ -631,7 +627,7 @@
   std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>>
     objects_of_interest_marker_interface_ptr_map_;
 
-  std::shared_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;
+  mutable SteeringFactorInterface steering_factor_interface_;
 
   mutable std::optional<Pose> stop_pose_{std::nullopt};