refactor(trajectory_modifier): refactor trajectory_modifier node

perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp

@@ -162,7 +162,8 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode(
 }
 
 void PointcloudBasedOccupancyGridMapNode::onPointcloudApproximateSync(
-  const PointCloud2::ConstSharedPtr & input_obstacle_msg, const PointCloud2::ConstSharedPtr & input_raw_msg)
+  const PointCloud2::ConstSharedPtr & input_obstacle_msg,
+  const PointCloud2::ConstSharedPtr & input_raw_msg)
 {
   // Keep the existing async upload/compute model, but trigger processing at approx-synced arrival.
   obstacle_pointcloud_.fromROSMsgAsync(input_obstacle_msg);

perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp

@@ -26,15 +26,15 @@
 #include <autoware_utils/system/time_keeper.hpp>
 #include <builtin_interfaces/msg/time.hpp>
 #include <laser_geometry/laser_geometry.hpp>
-#include <message_filters/subscriber.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/synchronizer.h>
 #include <rclcpp/rclcpp.hpp>
 
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 
 #include <cuda_runtime.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 

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;

planning/autoware_trajectory_modifier/launch/trajectory_modifier.launch.xml

@@ -4,9 +4,19 @@
   <arg name="input_odometry" default="/localization/kinematic_state"/>
   <arg name="input_acceleration" default="/localization/acceleration"/>
   <arg name="output_candidate_trajectories" default="~/output/candidate_trajectories"/>
+  <arg name="launch_stop_point_fixer" default="true"/>
+  <arg name="launch_modules_list_end" default="&quot;&quot;]"/>
+  <arg name="trajectory_modifier_launch_modules" default="["/>
+  <let
+    name="trajectory_modifier_launch_modules"
+    value="$(eval &quot;'$(var trajectory_modifier_launch_modules)' + 'autoware::trajectory_modifier::plugin::StopPointFixer, '&quot;)"
+    if="$(var launch_stop_point_fixer)"
+  />
+  <let name="trajectory_modifier_launch_modules" value="$(eval &quot;'$(var trajectory_modifier_launch_modules)' + '$(var launch_modules_list_end)'&quot;)"/>
 
   <node pkg="autoware_trajectory_modifier" exec="autoware_trajectory_modifier_node" name="trajectory_modifier" output="screen">
     <param from="$(var trajectory_modifier_param_path)"/>
+    <param name="launch_modules" value="$(var trajectory_modifier_launch_modules)"/>
     <remap from="~/input/candidate_trajectories" to="$(var input_candidate_trajectories)"/>
     <remap from="~/input/odometry" to="$(var input_odometry)"/>
     <remap from="~/input/acceleration" to="$(var input_acceleration)"/>

planning/autoware_trajectory_modifier/package.xml

@@ -22,8 +22,10 @@
   <depend>autoware_utils</depend>
   <depend>autoware_utils_debug</depend>
   <depend>autoware_utils_rclcpp</depend>
+  <depend>generate_parameter_library</depend>
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
 

planning/autoware_trajectory_modifier/param/trajectory_modifier_parameter_struct.yaml

@@ -0,0 +1,22 @@
+trajectory_modifier_params:
+  use_stop_point_fixer:
+    type: bool
+    default_value: false
+    description: Enable the use of stop point fixer
+    read_only: false
+
+  stop_point_fixer:
+    velocity_threshold_mps:
+      type: double
+      default_value: 0.1
+      description: Velocity threshold below which ego vehicle is considered stationary [m/s]
+      read_only: false
+      validation:
+        gt_eq<>: [0.0]
+    min_distance_threshold_m:
+      type: double
+      default_value: 1.0
+      description: Minimum distance threshold to trigger trajectory replacement [m]
+      read_only: false
+      validation:
+        gt_eq<>: [0.0]

planning/autoware_trajectory_modifier/plugins.xml

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<library path="autoware_trajectory_modifier_plugins">
+  <class type="autoware::trajectory_modifier::plugin::StopPointFixer"
+         base_class_type="autoware::trajectory_modifier::plugin::TrajectoryModifierPluginBase">
+    <description>
+      Modifies the stop point of a trajectory to align with the obstacle stop point
+    </description>
+  </class>
+</library>

planning/autoware_trajectory_modifier/schema/trajectory_modifier.schema.json

@@ -36,7 +36,7 @@
     }
   },
   "properties": {
-    "/trajectory_modifier": {
+    "/**": {
       "type": "object",
       "properties": {
         "ros__parameters": {
@@ -47,6 +47,6 @@
       "additionalProperties": false
     }
   },
-  "required": ["/trajectory_modifier"],
+  "required": ["/**"],
   "additionalProperties": false
 }