refactor(bpp): rework steering factor interface (autowarefoundation#9325

) * refactor(bpp): rework steering factor interface Signed-off-by: satoshi-ota <[email protected]> * refactor(soa): rework steering factor interface Signed-off-by: satoshi-ota <[email protected]> * refactor(AbLC): rework steering factor interface Signed-off-by: satoshi-ota <[email protected]> * refactor(doa): rework steering factor interface Signed-off-by: satoshi-ota <[email protected]> * refactor(lc): rework steering factor interface Signed-off-by: satoshi-ota <[email protected]> * refactor(gp): rework steering factor interface Signed-off-by: satoshi-ota <[email protected]> * refactor(sp): rework steering factor interface Signed-off-by: satoshi-ota <[email protected]> * refactor(sbp): rework steering factor interface Signed-off-by: satoshi-ota <[email protected]> * refactor(ss): rework steering factor interface Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
a-maumau · Nov 15, 2024 · d819a66 · d819a66
1 parent 8028e38
commit d819a66
Show file tree

Hide file tree

Showing 33 changed files with 136 additions and 156 deletions.
diff --git a/...ior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/...ior_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)}
 {
 }

diff --git a/...ior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/...ior_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;
 

diff --git a/...avior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/...avior_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

diff --git a/...dance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/...dance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp
@@ -42,7 +42,7 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface
   {
     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;

diff --git a/...oidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/...oidance_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
   {

diff --git a/...avior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/...avior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp
@@ -329,9 +329,8 @@ DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule(
   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,

diff --git a/...r_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/...r_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_));
 }
 

diff --git a/...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/...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()
   {

diff --git a/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -63,9 +63,8 @@ GoalPlannerModule::GoalPlannerModule(
   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 @@ GoalPlannerModule::GoalPlannerModule(
     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 @@ void GoalPlannerModule::updateSteeringFactor(
     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()

diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
@@ -31,7 +31,7 @@ std::unique_ptr<SceneModuleInterface> GoalPlannerModuleManager::createNewSceneMo
 {
   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(

diff --git a/...r_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/...r_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;

diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/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
diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/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_));
 }
 

diff --git a/...havior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/...havior_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_

diff --git a/...g/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/...g/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -52,6 +52,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   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_ =
@@ -114,7 +116,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
       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 @@ void BehaviorPathPlannerNode::publish_steering_factor(
     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

diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp
@@ -177,6 +177,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
   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 @@ BehaviorModuleOutput SubPlannerManager::run(
 
   module_ptr->updateCurrentState();
 
-  module_ptr->publishSteeringFactor();
-
   module_ptr->publishObjectsOfInterestMarker();
 
   processing_time_.at(module_ptr->name()) += stop_watch.toc(module_ptr->name(), true);

diff --git a/...common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/...common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp
@@ -74,6 +74,7 @@ using tier4_rtc_msgs::msg::State;
 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 @@ class SceneModuleInterface
     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 @@ class SceneModuleInterface
     clearWaitingApproval();
     unlockNewModuleLaunch();
     unlockOutputPath();
-    steering_factor_interface_ptr_->clearSteeringFactors();
+
+    reset_factor();
 
     stop_reason_ = StopReason();
 
@@ -205,14 +205,6 @@ class SceneModuleInterface
     }
   }
 
-  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 @@ class SceneModuleInterface
 
   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 @@ class SceneModuleInterface
   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};