fix(behavior_path_planner): remove velocity factor

Signed-off-by: satoshi-ota <[email protected]>
tier4 · Dec 26, 2024 · 2e32809 · 2e32809
1 parent 153c9c8
commit 2e32809
Show file tree

Hide file tree

Showing 6 changed files with 0 additions and 146 deletions.
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
@@ -21,7 +21,6 @@
 #include "planner_manager.hpp"
 
 #include <autoware/motion_utils/factor/planning_factor_interface.hpp>
-#include <autoware/motion_utils/factor/steering_factor_interface.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
 
@@ -53,7 +52,6 @@
 namespace autoware::behavior_path_planner
 {
 using autoware::motion_utils::PlanningFactorInterface;
-using autoware::motion_utils::SteeringFactorInterface;
 using autoware_adapi_v1_msgs::msg::OperationModeState;
 using autoware_map_msgs::msg::LaneletMapBin;
 using autoware_perception_msgs::msg::PredictedObject;
@@ -123,7 +121,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
   rclcpp::Publisher<PoseWithUuidStamped>::SharedPtr modified_goal_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 +135,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
 
   std::shared_ptr<PlannerManager> planner_manager_;
 
-  SteeringFactorInterface steering_factor_interface_;
   std::unique_ptr<PlanningFactorInterface> planning_factor_interface_;
 
   std::mutex mutex_pd_;       // mutex for planner_data_

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
@@ -53,8 +53,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   const auto durable_qos = rclcpp::QoS(1).transient_local();
   modified_goal_publisher_ =
     create_publisher<PoseWithUuidStamped>("~/output/modified_goal", durable_qos);
-  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_ =
@@ -117,8 +115,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
       turn_signal_search_time, turn_signal_intersection_angle_threshold_deg);
   }
 
-  steering_factor_interface_.init(PlanningBehavior::INTERSECTION);
-
   // Start timer
   {
     const auto planning_hz = declare_parameter<double>("planning_hz");
@@ -465,20 +461,9 @@ void BehaviorPathPlannerNode::publish_steering_factor(
   const auto [intersection_flag, approaching_intersection_flag] =
     planner_data->turn_signal_decider.getIntersectionTurnSignalFlag();
   if (intersection_flag || approaching_intersection_flag) {
-    const uint16_t steering_factor_direction = std::invoke([&turn_signal]() {
-      if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
-        return SteeringFactor::LEFT;
-      }
-      return SteeringFactor::RIGHT;
-    });
-
     const auto [intersection_pose, intersection_distance] =
       planner_data->turn_signal_decider.getIntersectionPoseAndDistance();
 
-    steering_factor_interface_.set(
-      {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance},
-      steering_factor_direction, SteeringFactor::TURNING, "");
-
     const uint16_t planning_factor_direction = std::invoke([&turn_signal]() {
       if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
         return PlanningFactor::TURN_LEFT;
@@ -489,21 +474,8 @@ void BehaviorPathPlannerNode::publish_steering_factor(
     planning_factor_interface_->add(
       intersection_distance, intersection_distance, intersection_pose, intersection_pose,
       planning_factor_direction, SafetyFactorArray{});
-  } else {
-    steering_factor_interface_.reset();
   }
 
-  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);
-
   planning_factor_interface_->publish();
 }
 

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
@@ -182,8 +182,6 @@ 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();
-    m->publishVelocityFactor();
     m->publish_planning_factors();
   });
 

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
@@ -22,8 +22,6 @@
 
 #include <autoware/behavior_path_planner_common/turn_signal_decider.hpp>
 #include <autoware/motion_utils/factor/planning_factor_interface.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>
@@ -37,7 +35,6 @@
 #include <magic_enum.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <autoware_adapi_v1_msgs/msg/steering_factor.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_rtc_msgs/msg/state.hpp>
@@ -56,16 +53,11 @@
 namespace autoware::behavior_path_planner
 {
 using autoware::motion_utils::PlanningFactorInterface;
-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::PlanningFactor;
@@ -106,26 +98,6 @@ class SceneModuleInterface
     }
   }
 
-  // TODO(satoshi-ota): remove this constructor after all planning factors have been migrated.
-  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)
-  : 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)),
-    planning_factor_interface_{std::make_shared<PlanningFactorInterface>(&node, "tmp")},
-    time_keeper_(std::make_shared<universe_utils::TimeKeeper>())
-  {
-    for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
-      uuid_map_.emplace(module_name, generateUUID());
-    }
-  }
-
   SceneModuleInterface(const SceneModuleInterface &) = delete;
   SceneModuleInterface(SceneModuleInterface &&) = delete;
   SceneModuleInterface & operator=(const SceneModuleInterface &) = delete;
@@ -210,8 +182,6 @@ class SceneModuleInterface
     unlockNewModuleLaunch();
     unlockOutputPath();
 
-    reset_factor();
-
     processOnExit();
   }
 
@@ -284,16 +254,6 @@ class SceneModuleInterface
 
   ModuleStatus getCurrentStatus() const { return current_state_; }
 
-  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_; }
 
   PoseWithDetailOpt getStopPose() const
@@ -584,22 +544,6 @@ class SceneModuleInterface
     }
   }
 
-  void setVelocityFactor(const PathWithLaneId & path)
-  {
-    if (stop_pose_.has_value()) {
-      velocity_factor_interface_.set(
-        path.points, getEgoPose(), stop_pose_.value().pose, VelocityFactor::APPROACHING, "stop");
-      return;
-    }
-
-    if (!slow_pose_.has_value()) {
-      return;
-    }
-
-    velocity_factor_interface_.set(
-      path.points, getEgoPose(), slow_pose_.value().pose, VelocityFactor::APPROACHING, "slow down");
-  }
-
   void set_longitudinal_planning_factor(const PathWithLaneId & path)
   {
     if (stop_pose_.has_value()) {
@@ -673,10 +617,6 @@ class SceneModuleInterface
 
   mutable std::shared_ptr<PlanningFactorInterface> planning_factor_interface_;
 
-  mutable SteeringFactorInterface steering_factor_interface_;
-
-  mutable VelocityFactorInterface velocity_factor_interface_;
-
   mutable PoseWithDetailOpt stop_pose_{std::nullopt};
 
   mutable PoseWithDetailOpt slow_pose_{std::nullopt};

diff --git a/...nclude/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/...nclude/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
@@ -24,8 +24,6 @@
 #include <rclcpp/parameter.hpp>
 #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,8 +40,6 @@ using autoware::motion_utils::createDeadLineVirtualWallMarker;
 using autoware::motion_utils::createSlowDownVirtualWallMarker;
 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>;
@@ -106,46 +102,6 @@ class SceneModuleManagerInterface
     }
   }
 
-  void publishSteeringFactor()
-  {
-    SteeringFactorArray steering_factor_array;
-    steering_factor_array.header.frame_id = "map";
-    steering_factor_array.header.stamp = node_->now();
-
-    for (const auto & m : observers_) {
-      if (m.expired()) {
-        continue;
-      }
-
-      const auto steering_factor = m.lock()->get_steering_factor();
-      if (steering_factor.behavior != PlanningBehavior::UNKNOWN) {
-        steering_factor_array.factors.emplace_back(steering_factor);
-      }
-    }
-
-    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 publish_planning_factors() { planning_factor_interface_->publish(); }
 
   void publishVirtualWall() const
@@ -306,10 +262,6 @@ class SceneModuleManagerInterface
 
   rclcpp::Publisher<MarkerArray>::SharedPtr pub_drivable_lanes_;
 
-  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_;

diff --git a/...er/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp b/...er/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp
@@ -61,10 +61,6 @@ void SceneModuleManagerInterface::initInterface(
     pub_drivable_lanes_ = node->create_publisher<MarkerArray>("~/drivable_lanes/" + name_, 20);
     pub_processing_time_ = node->create_publisher<universe_utils::ProcessingTimeDetail>(
       "~/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);
   }
 
   // planning factor