Merge pull request #2695 from tier4/feat/add_modifier_planning_factor

feat(trajectory_modifier): add PlanningFactor interface

Author: Kotakku

planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_plugins/trajectory_modifier_plugin_base.hpp

@@ -18,6 +18,7 @@
 #define AUTOWARE__TRAJECTORY_MODIFIER__TRAJECTORY_MODIFIER_PLUGINS__TRAJECTORY_MODIFIER_PLUGIN_BASE_HPP_
 #include "autoware/trajectory_modifier/trajectory_modifier_structs.hpp"
 
+#include <autoware/planning_factor_interface/planning_factor_interface.hpp>
 #include <autoware_utils_debug/time_keeper.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -32,6 +33,7 @@
 
 namespace autoware::trajectory_modifier::plugin
 {
+using autoware_internal_planning_msgs::msg::PlanningFactor;
 using autoware_planning_msgs::msg::TrajectoryPoint;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
 
@@ -61,6 +63,24 @@ class TrajectoryModifierPluginBase
   rclcpp::Node * get_node_ptr() const { return node_ptr_; }
   std::shared_ptr<autoware_utils_debug::TimeKeeper> get_time_keeper() const { return time_keeper_; }
 
+  virtual void publish_planning_factor()
+  {
+    if (planning_factor_interface_) {
+      planning_factor_interface_->publish();
+    }
+  }
+  std::vector<PlanningFactor> get_planning_factors() const
+  {
+    if (planning_factor_interface_ != nullptr) {
+      return planning_factor_interface_->get_factors();
+    }
+    return {};
+  }
+
+protected:
+  std::unique_ptr<autoware::planning_factor_interface::PlanningFactorInterface>
+    planning_factor_interface_;
+
 private:
   std::string name_;
   rclcpp::Node * node_ptr_;

planning/autoware_trajectory_modifier/package.xml

@@ -17,6 +17,7 @@
 
   <depend>autoware_internal_planning_msgs</depend>
   <depend>autoware_motion_utils</depend>
+  <depend>autoware_planning_factor_interface</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_utils</depend>
   <depend>autoware_utils_debug</depend>

planning/autoware_trajectory_modifier/src/trajectory_modifier.cpp

@@ -69,6 +69,7 @@ void TrajectoryModifier::on_traj(const CandidateTrajectories::ConstSharedPtr msg
   for (auto & trajectory : output_trajectories.candidate_trajectories) {
     for (auto & modifier : modifier_plugins_) {
       modifier->modify_trajectory(trajectory.points, params_, data_);
+      modifier->publish_planning_factor();
     }
   }
 

planning/autoware_trajectory_modifier/src/trajectory_modifier_plugins/stop_point_fixer.cpp

@@ -33,6 +33,9 @@ StopPointFixer::StopPointFixer(
   const TrajectoryModifierParams & params)
 : TrajectoryModifierPluginBase(name, node_ptr, time_keeper, params)
 {
+  planning_factor_interface_ =
+    std::make_unique<autoware::planning_factor_interface::PlanningFactorInterface>(
+      node_ptr, "stop_point_fixer");
   set_up_params();
 }
 
@@ -61,6 +64,13 @@ void StopPointFixer::modify_trajectory(
 {
   if (is_trajectory_modification_required(traj_points, params, data)) {
     utils::replace_trajectory_with_stop_point(traj_points, data.current_odometry.pose.pose);
+
+    // Add PlanningFactor for the stop decision
+    const auto & ego_pose = data.current_odometry.pose.pose;
+    planning_factor_interface_->add(
+      traj_points, ego_pose, ego_pose, PlanningFactor::STOP,
+      autoware_internal_planning_msgs::msg::SafetyFactorArray{});
+
     auto clock_ptr = get_node_ptr()->get_clock();
     RCLCPP_DEBUG_THROTTLE(
       get_node_ptr()->get_logger(), *clock_ptr, 5000,