Merge branch 'feat/v0.48/e2e' into feat/v0.48/enable_dp_traffic_light

Author: Owen-Liuyuxuan

common/autoware_boundary_departure_checker/include/autoware/boundary_departure_checker/parameters.hpp

@@ -147,7 +147,7 @@ struct Param
   double critical_departure_on_time_buffer_s{0.15};
   double critical_departure_off_time_buffer_s{0.15};
   AbnormalitiesConfigs abnormality_configs;
-  std::vector<std::string> boundary_types_to_detect;
+  std::vector<std::string> boundary_types_to_detect{"road_border"};
   std::vector<FootprintType> footprint_types_to_check;
 
   template <typename ConfigType>

common/autoware_boundary_departure_checker/include/autoware/boundary_departure_checker/utils.hpp

@@ -257,17 +257,13 @@ UncrossableBoundRTree build_uncrossable_boundaries_rtree(
  * If it is within the segment's bounds, it returns the projection point. If it's outside,
  * an error is returned indicating the relative position.
  *
- * The `swap_points` flag controls the order of the returned pair, useful when projection
- * direction matters (e.g., from ego to boundary vs. from boundary to ego).
- *
  * @param p            The point to be projected.
  * @param segment      The line segment to project onto.
- * @param swap_points  Whether to swap the order of the original and projected point in the result.
  * @return A tuple containing the original point, projection point, and the distance between them,
  *         or an error message if the point lies outside the segment.
  */
-tl::expected<std::tuple<Point2d, Point2d, double>, std::string> point_to_segment_projection(
-  const Point2d & p, const Segment2d & segment, const bool swap_points = false);
+tl::expected<std::pair<Point2d, double>, std::string> point_to_segment_projection(
+  const Point2d & p, const Segment2d & segment);
 
 /**
  * @brief Computes the nearest projection between two segments, used to assess lateral distance.

common/autoware_boundary_departure_checker/src/uncrossable_boundary_departure_checker.cpp

@@ -297,6 +297,12 @@ tl::expected<DepartureData, std::string> UncrossableBoundaryDepartureChecker::ge
     trimmed_pred_traj, *vehicle_info_ptr_, curr_pose_with_cov, param_);
   const auto & footprint_type_order = footprint_manager_->get_footprint_type_order();
 
+  if (
+    generated_footprints.empty() || footprint_type_order.empty() ||
+    generated_footprints.at(footprint_type_order.front()).empty()) {
+    return tl::make_unexpected("Failed to generate any footprints for abnormalities");
+  }
+
   if (generated_footprints.empty() || footprint_type_order.empty()) {
     return tl::make_unexpected("Failed to generate any footprints");
   }

common/autoware_boundary_departure_checker/src/utils.cpp

@@ -418,35 +418,25 @@ UncrossableBoundRTree build_uncrossable_boundaries_rtree(
   return {segments.begin(), segments.end()};
 }
 
-tl::expected<std::tuple<Point2d, Point2d, double>, std::string> point_to_segment_projection(
-  const Point2d & p, const Segment2d & segment, const bool swap_points)
+tl::expected<std::pair<Point2d, double>, std::string> point_to_segment_projection(
+  const Point2d & p, const Segment2d & segment)
 {
   const auto & p1 = segment.first;
   const auto & p2 = segment.second;
 
   const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()};
   const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()};
 
-  const auto result = [&swap_points](
-                        const Point2d & orig,
-                        const Point2d & proj) -> std::tuple<Point2d, Point2d, double> {
-    return swap_points ? std::make_tuple(proj, orig, boost::geometry::distance(proj, orig))
-                       : std::make_tuple(orig, proj, boost::geometry::distance(orig, proj));
-  };
-
   const auto c1 = boost::geometry::dot_product(p_vec, p2_vec);
   if (c1 < 0.0) return tl::make_unexpected("Point before segment start");
-  if (c1 == 0.0) return result(p, p1);
 
   const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec);
   if (c1 > c2) return tl::make_unexpected("Point after segment end");
 
-  if (c1 == c2) return result(p, p2);
-
   const auto projection = p1 + (p2_vec * c1 / c2);
   const auto projection_point = Point2d{projection.x(), projection.y()};
 
-  return result(p, projection_point);
+  return std::make_pair(projection_point, boost::geometry::distance(p, projection_point));
 }
 
 tl::expected<ProjectionToBound, std::string> segment_to_segment_nearest_projection(
@@ -465,29 +455,28 @@ tl::expected<ProjectionToBound, std::string> segment_to_segment_nearest_projecti
 
   ProjectionsToBound projections;
   projections.reserve(4);
-  constexpr bool swap_result = true;
-  if (const auto projection_opt = point_to_segment_projection(ego_f, lane_seg, swap_result)) {
-    const auto & [pt_ego, pt_lane, dist] = *projection_opt;
-    const auto lon_offset = boost::geometry::distance(pt_ego, ego_f);
-    projections.emplace_back(pt_ego, pt_lane, lane_seg, dist, lon_offset, ego_sides_idx);
+  if (const auto projection_opt = point_to_segment_projection(ego_f, lane_seg)) {
+    const auto & [proj, dist] = *projection_opt;
+    constexpr auto lon_offset = 0.0;
+    projections.emplace_back(ego_f, proj, lane_seg, dist, lon_offset, ego_sides_idx);
   }
 
-  if (const auto projection_opt = point_to_segment_projection(ego_b, lane_seg, swap_result)) {
-    const auto & [pt_ego, pt_lane, dist] = *projection_opt;
-    const auto lon_offset = boost::geometry::distance(pt_ego, ego_f);
-    projections.emplace_back(pt_ego, pt_lane, lane_seg, dist, lon_offset, ego_sides_idx);
+  if (const auto projection_opt = point_to_segment_projection(ego_b, lane_seg)) {
+    const auto & [proj, dist] = *projection_opt;
+    const auto lon_offset = boost::geometry::distance(ego_b, ego_f);
+    projections.emplace_back(ego_b, proj, lane_seg, dist, lon_offset, ego_sides_idx);
   }
 
-  if (const auto projection_opt = point_to_segment_projection(lane_pt1, ego_seg, !swap_result)) {
-    const auto & [pt_ego, pt_lane, dist] = *projection_opt;
-    const auto lon_offset = boost::geometry::distance(pt_ego, ego_f);
-    projections.emplace_back(pt_ego, pt_lane, lane_seg, dist, lon_offset, ego_sides_idx);
+  if (const auto projection_opt = point_to_segment_projection(lane_pt1, ego_seg)) {
+    const auto & [proj, dist] = *projection_opt;
+    const auto lon_offset = boost::geometry::distance(proj, ego_f);
+    projections.emplace_back(proj, lane_pt1, lane_seg, dist, lon_offset, ego_sides_idx);
   }
 
-  if (const auto projection_opt = point_to_segment_projection(lane_pt2, ego_seg, !swap_result)) {
-    const auto & [pt_ego, pt_lane, dist] = *projection_opt;
-    const auto lon_offset = boost::geometry::distance(pt_ego, ego_f);
-    projections.emplace_back(pt_ego, pt_lane, lane_seg, dist, lon_offset, ego_sides_idx);
+  if (const auto projection_opt = point_to_segment_projection(lane_pt2, ego_seg)) {
+    const auto & [proj, dist] = *projection_opt;
+    const auto lon_offset = boost::geometry::distance(proj, ego_f);
+    projections.emplace_back(proj, lane_pt2, lane_seg, dist, lon_offset, ego_sides_idx);
   }
 
   if (projections.empty())

planning/autoware_trajectory_modifier/CMakeLists.txt

@@ -3,18 +3,37 @@ project(autoware_trajectory_modifier)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
+pluginlib_export_plugin_description_file(autoware_trajectory_modifier plugins.xml)
 
-ament_auto_add_library(autoware_trajectory_modifier_component SHARED
+generate_parameter_library(trajectory_modifier_param
+  param/trajectory_modifier_parameter_struct.yaml
+)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/trajectory_modifier.cpp
   src/utils.cpp
-  src/trajectory_modifier_plugins/stop_point_fixer.cpp
 )
 
-target_link_libraries(autoware_trajectory_modifier_component)
+target_link_libraries(${PROJECT_NAME}
+  trajectory_modifier_param
+)
+
+target_compile_options(${PROJECT_NAME} PUBLIC
+  -Wno-error=deprecated-declarations
+)
 
-rclcpp_components_register_node(autoware_trajectory_modifier_component
+rclcpp_components_register_node(${PROJECT_NAME}
   PLUGIN "autoware::trajectory_modifier::TrajectoryModifier"
-  EXECUTABLE autoware_trajectory_modifier_node
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_add_library(${PROJECT_NAME}_plugins SHARED
+  src/trajectory_modifier_plugins/stop_point_fixer.cpp
+)
+
+target_link_libraries(${PROJECT_NAME}_plugins
+  ${PROJECT_NAME}
+  trajectory_modifier_param
 )
 
 if(BUILD_TESTING)
@@ -26,7 +45,8 @@ if(BUILD_TESTING)
     ${TEST_SOURCES}
   )
   target_link_libraries(test_autoware_trajectory_modifier
-    autoware_trajectory_modifier_component
+    ${PROJECT_NAME}
+    ${PROJECT_NAME}_plugins
   )
 endif()
 

planning/autoware_trajectory_modifier/config/trajectory_modifier.param.yaml

@@ -1,4 +1,4 @@
-/trajectory_modifier:
+/**:
   ros__parameters:
     use_stop_point_fixer: true
 

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

@@ -19,8 +19,10 @@
 #include "autoware/trajectory_modifier/trajectory_modifier_plugins/trajectory_modifier_plugin_base.hpp"
 #include "autoware/trajectory_modifier/trajectory_modifier_structs.hpp"
 
+#include <autoware_trajectory_modifier/trajectory_modifier_param.hpp>
 #include <autoware_utils_debug/time_keeper.hpp>
 #include <autoware_utils_rclcpp/polling_subscriber.hpp>
+#include <pluginlib/class_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/subscription.hpp>
 
@@ -51,16 +53,16 @@ class TrajectoryModifier : public rclcpp::Node
 
 private:
   void on_traj(const CandidateTrajectories::ConstSharedPtr msg);
-  void set_up_params();
   void initialize_modifiers();
+  void load_plugin(const std::string & name);
+  void unload_plugin(const std::string & name);
   void reset_previous_data();
   bool initialized_modifiers_{false};
 
-  rcl_interfaces::msg::SetParametersResult on_parameter(
-    const std::vector<rclcpp::Parameter> & parameters);
+  std::unique_ptr<trajectory_modifier_params::ParamListener> param_listener_;
+  trajectory_modifier_params::Params params_;
 
-  std::vector<std::shared_ptr<plugin::TrajectoryModifierPluginBase>> modifier_plugins_;
-  std::shared_ptr<plugin::StopPointFixer> stop_point_fixer_ptr_;
+  void update_params();
 
   rclcpp::Subscription<CandidateTrajectories>::SharedPtr trajectories_sub_;
   rclcpp::Publisher<CandidateTrajectories>::SharedPtr trajectories_pub_;
@@ -79,9 +81,10 @@ class TrajectoryModifier : public rclcpp::Node
     debug_processing_time_detail_pub_;
   mutable std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper_{nullptr};
 
-  TrajectoryModifierParams params_;
+  pluginlib::ClassLoader<plugin::TrajectoryModifierPluginBase> plugin_loader_;
+  std::vector<std::shared_ptr<plugin::TrajectoryModifierPluginBase>> plugins_;
+
   TrajectoryModifierData data_;
-  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
 };
 
 }  // namespace autoware::trajectory_modifier

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

@@ -27,29 +27,27 @@ namespace autoware::trajectory_modifier::plugin
 class StopPointFixer : public TrajectoryModifierPluginBase
 {
 public:
-  StopPointFixer(
-    const std::string & name, rclcpp::Node * node_ptr,
-    const std::shared_ptr<autoware_utils_debug::TimeKeeper> & time_keeper,
-    const TrajectoryModifierParams & params);
+  StopPointFixer() = default;
 
   void modify_trajectory(
-    TrajectoryPoints & traj_points, const TrajectoryModifierParams & params,
-    const TrajectoryModifierData & data) override;
-  void set_up_params() override;
-  rcl_interfaces::msg::SetParametersResult on_parameter(
-    const std::vector<rclcpp::Parameter> & parameters) override;
-  bool is_trajectory_modification_required(
-    const TrajectoryPoints & traj_points, const TrajectoryModifierParams & params,
-    const TrajectoryModifierData & data) const override;
+    TrajectoryPoints & traj_points, const TrajectoryModifierData & data) override;
 
-private:
-  struct Parameters
+  [[nodiscard]] bool is_trajectory_modification_required(
+    const TrajectoryPoints & traj_points, const TrajectoryModifierData & data) const override;
+
+  void update_params(const TrajectoryModifierParams & params) override
   {
-    double velocity_threshold_mps{0.1};
-    double min_distance_threshold_m{1.0};
-  };
+    params_ = params.stop_point_fixer;
+    enabled_ = params.use_stop_point_fixer;
+  }
+
+  const TrajectoryModifierParams::StopPointFixer & get_params() const { return params_; }
 
-  Parameters params_;
+protected:
+  void on_initialize(const TrajectoryModifierParams & params) override;
+
+private:
+  TrajectoryModifierParams::StopPointFixer params_;
 };
 
 }  // namespace autoware::trajectory_modifier::plugin

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

@@ -19,6 +19,7 @@
 #include "autoware/trajectory_modifier/trajectory_modifier_structs.hpp"
 
 #include <autoware/planning_factor_interface/planning_factor_interface.hpp>
+#include <autoware_trajectory_modifier/trajectory_modifier_param.hpp>
 #include <autoware_utils_debug/time_keeper.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -36,32 +37,35 @@ namespace autoware::trajectory_modifier::plugin
 using autoware_internal_planning_msgs::msg::PlanningFactor;
 using autoware_planning_msgs::msg::TrajectoryPoint;
 using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using TrajectoryModifierParams = trajectory_modifier_params::Params;
 
 class TrajectoryModifierPluginBase
 {
 public:
-  TrajectoryModifierPluginBase(
+  TrajectoryModifierPluginBase() = default;
+
+  void initialize(
     std::string name, rclcpp::Node * node_ptr,
     const std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper,
     [[maybe_unused]] const TrajectoryModifierParams & params)
-  : name_(std::move(name)), node_ptr_(node_ptr), time_keeper_(time_keeper)
   {
+    name_ = std::move(name);
+    node_ptr_ = node_ptr;
+    time_keeper_ = time_keeper;
     RCLCPP_DEBUG(
       node_ptr_->get_logger(), "instantiated TrajectoryModifierPluginBase: %s", name_.c_str());
+    on_initialize(params);
   }
+
   virtual ~TrajectoryModifierPluginBase() = default;
   virtual void modify_trajectory(
-    TrajectoryPoints & traj_points, const TrajectoryModifierParams & params,
-    const TrajectoryModifierData & data) = 0;
-  virtual void set_up_params() = 0;
-  virtual rcl_interfaces::msg::SetParametersResult on_parameter(
-    const std::vector<rclcpp::Parameter> & parameters) = 0;
+    TrajectoryPoints & traj_points, const TrajectoryModifierData & data) = 0;
   virtual bool is_trajectory_modification_required(
-    const TrajectoryPoints & traj_points, const TrajectoryModifierParams & params,
-    const TrajectoryModifierData & data) const = 0;
+    const TrajectoryPoints & traj_points, const TrajectoryModifierData & data) const = 0;
   std::string get_name() const { return name_; }
   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 update_params(const TrajectoryModifierParams & params) = 0;
 
   virtual void publish_planning_factor()
   {
@@ -78,8 +82,10 @@ class TrajectoryModifierPluginBase
   }
 
 protected:
+  virtual void on_initialize(const TrajectoryModifierParams & params) = 0;
   std::unique_ptr<autoware::planning_factor_interface::PlanningFactorInterface>
     planning_factor_interface_;
+  bool enabled_{true};
 
 private:
   std::string name_;

planning/autoware_trajectory_modifier/include/autoware/trajectory_modifier/trajectory_modifier_structs.hpp

@@ -22,11 +22,6 @@ namespace autoware::trajectory_modifier
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using nav_msgs::msg::Odometry;
 
-struct TrajectoryModifierParams
-{
-  bool use_stop_point_fixer{true};
-};
-
 struct TrajectoryModifierData
 {
   Odometry current_odometry;