feat: combine concatenator with validator

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

planning/autoware_trajectory_concatenator/CMakeLists.txt

@@ -20,10 +20,4 @@ target_link_libraries(${PROJECT_NAME}
   ${PROJECT_NAME}_param
 )
 
-target_compile_options(${PROJECT_NAME} PUBLIC
-  -Wno-error=deprecated-declarations
-)
-
-ament_auto_package(
-  INSTALL_TO_SHARE
-)
+ament_auto_package()

planning/autoware_trajectory_validator/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.14)
 project(autoware_trajectory_validator)
 
 find_package(autoware_cmake REQUIRED)
+find_package(autoware_trajectory_concatenator REQUIRED)
 autoware_package()
 pluginlib_export_plugin_description_file(autoware_trajectory_validator plugins.xml)
 
@@ -24,6 +25,10 @@ target_link_libraries(${trajectory_validator_lib_target}
   ${PROJECT_NAME}_param
 )
 
+ament_target_dependencies(${trajectory_validator_lib_target}
+  autoware_trajectory_concatenator
+)
+
 target_compile_options(${trajectory_validator_lib_target} PUBLIC
   -Wno-error=deprecated-declarations
 )

planning/autoware_trajectory_validator/include/autoware/trajectory_validator/parameters.hpp

@@ -0,0 +1,62 @@
+// Copyright 2026 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef AUTOWARE__TRAJECTORY_VALIDATOR__PARAMETERS_HPP_
+#define AUTOWARE__TRAJECTORY_VALIDATOR__PARAMETERS_HPP_
+
+#include <autoware_trajectory_concatenator/autoware_trajectory_concatenator_param.hpp>
+#include <autoware_trajectory_validator/autoware_trajectory_validator_param.hpp>
+#include <rclcpp/node_interfaces/node_parameters_interface.hpp>
+
+namespace autoware::trajectory_validator
+{
+struct TrajectoryValidatorParam
+{
+  explicit TrajectoryValidatorParam(
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface)
+  : validator_params_listener(node_parameters_interface),
+    concatenator_params_listener(node_parameters_interface)
+  {
+    validator_params = validator_params_listener.get_params();
+    concatenator_params = concatenator_params_listener.get_params();
+  }
+
+  validator::ParamListener validator_params_listener;
+  validator::Params validator_params;
+  concatenator::ParamListener concatenator_params_listener;
+  concatenator::Params concatenator_params;
+
+  std::optional<std::string> update_parameters()
+  {
+    std::string status;
+    if (validator_params_listener.is_old(validator_params)) {
+      validator_params = validator_params_listener.get_params();
+      status += "Validator parameters updated. ";
+    }
+
+    if (concatenator_params_listener.is_old(concatenator_params)) {
+      concatenator_params = concatenator_params_listener.get_params();
+      status += "Concatenator parameters updated. ";
+    }
+
+    if (status.empty()) {
+      return std::nullopt;
+    }
+
+    return status;
+  }
+};
+}  // namespace autoware::trajectory_validator
+
+#endif  // AUTOWARE__TRAJECTORY_VALIDATOR__PARAMETERS_HPP_

planning/autoware_trajectory_validator/include/autoware/trajectory_validator/trajectory_validator_node.hpp

@@ -15,12 +15,13 @@
 #ifndef AUTOWARE__TRAJECTORY_VALIDATOR__TRAJECTORY_VALIDATOR_NODE_HPP_
 #define AUTOWARE__TRAJECTORY_VALIDATOR__TRAJECTORY_VALIDATOR_NODE_HPP_
 
+#include "autoware/trajectory_concatenator/trajectory_concatenator.hpp"
 #include "autoware/trajectory_validator/evaluation_context.hpp"
 #include "autoware/trajectory_validator/validation_stage_report.hpp"
 #include "autoware/trajectory_validator/validator_interface.hpp"
 
 #include <autoware/lanelet2_utils/conversion.hpp>
-#include <autoware_trajectory_validator/autoware_trajectory_validator_param.hpp>
+#include <autoware/trajectory_validator/parameters.hpp>
 #include <autoware_trajectory_validator/msg/metric_report.hpp>
 #include <autoware_trajectory_validator/msg/validation_report.hpp>
 #include <autoware_trajectory_validator/msg/validation_report_array.hpp>
@@ -42,9 +43,8 @@
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
-#include <algorithm>
-#include <functional>
 #include <memory>
+#include <mutex>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -79,13 +79,16 @@ class TrajectoryValidator : public rclcpp::Node
    */
   void publishers();
 
+  void on_trajectories(const CandidateTrajectories::ConstSharedPtr msg);
+
+  void on_timer();
+
+  void execute_pipeline(const CandidateTrajectories & concatenated_trajectories);
   /**
    * @brief Gather the latest inputs required to run the filter plugins.
    */
   tl::expected<EvaluationContext, std::string> take_data();
 
-  void process(const CandidateTrajectories::ConstSharedPtr msg);
-
   void map_callback(const LaneletMapBin::ConstSharedPtr msg);
 
   void load_metric(const std::string & name, const bool is_shadow_mode = false);
@@ -138,8 +141,7 @@ class TrajectoryValidator : public rclcpp::Node
     const std::unordered_map<std::string, double> & processing_time);
 
   // Parameters
-  validator::ParamListener listener_;
-  validator::Params params_;
+  TrajectoryValidatorParam params_;
 
   // Plugin infrastructure
   pluginlib::ClassLoader<plugin::ValidatorInterface> plugin_loader_;
@@ -158,6 +160,12 @@ class TrajectoryValidator : public rclcpp::Node
   rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_map_;
   rclcpp::Subscription<CandidateTrajectories>::SharedPtr sub_trajectories_;
 
+  rclcpp::Subscription<CandidateTrajectories>::SharedPtr sub_trajectories_generative_;
+  rclcpp::Subscription<CandidateTrajectories>::SharedPtr sub_trajectories_backup_;
+
+  // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
   // Publishers
   rclcpp::Publisher<CandidateTrajectories>::SharedPtr pub_trajectories_;
   std::shared_ptr<autoware_utils_debug::DebugPublisher> pub_validation_reports_;
@@ -170,6 +178,10 @@ class TrajectoryValidator : public rclcpp::Node
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
   DiagnosticsInterface diagnostics_interface_{this, "trajectory_validator"};
   mutable std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper_{nullptr};
+
+  // Concatenator state
+  std::unique_ptr<trajectory_concatenator::TrajectoryConcatenator> concatenator_ptr_;
+  std::mutex buffer_mutex_;
 };
 
 }  // namespace autoware::trajectory_validator

planning/autoware_trajectory_validator/launch/trajectory_validator.launch.xml

@@ -1,7 +1,8 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <arg name="trajectory_validator_param_path" default="$(find-pkg-share autoware_trajectory_validator)/config/trajectory_validator.param.yaml"/>
-  <arg name="input_trajectories" default="~/input/trajectories"/>
+  <arg name="input_generative_trajectories" default="~/input/trajectories_generative"/>
+  <arg name="input_backup_trajectories" default="~/input/trajectories_backup"/>
   <arg name="output_trajectories" default="~/output/trajectories"/>
   <arg name="input_vector_map" default="~/input/vector_map"/>
   <arg name="input_odometry" default="~/input/odometry"/>
@@ -11,7 +12,8 @@
 
   <node pkg="autoware_trajectory_validator" exec="autoware_trajectory_validator_node" name="trajectory_validator_node" output="screen">
     <param from="$(var trajectory_validator_param_path)" allow_substs="true"/>
-    <remap from="~/input/trajectories" to="$(var input_trajectories)"/>
+    <remap from="~/input/trajectories_generative" to="$(var input_generative_trajectories)"/>
+    <remap from="~/input/trajectories_backup" to="$(var input_backup_trajectories)"/>
     <remap from="~/output/trajectories" to="$(var output_trajectories)"/>
     <remap from="~/input/lanelet2_map" to="$(var input_vector_map)"/>
     <remap from="~/input/odometry" to="$(var input_odometry)"/>

planning/autoware_trajectory_validator/package.xml

@@ -31,6 +31,7 @@
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_traffic_light_utils</depend>
+  <depend>autoware_trajectory_concatenator</depend>
   <depend>autoware_utils_debug</depend>
   <depend>autoware_utils_diagnostics</depend>
   <depend>autoware_utils_geometry</depend>

planning/autoware_trajectory_validator/src/trajectory_validator_node.cpp

@@ -39,19 +39,20 @@ namespace autoware::trajectory_validator
 
 TrajectoryValidator::TrajectoryValidator(const rclcpp::NodeOptions & options)
 : Node{"trajectory_validator_node", options},
-  listener_{get_node_parameters_interface()},
-  params_(listener_.get_params()),
+  params_{get_node_parameters_interface()},
   plugin_loader_(
     "autoware_trajectory_validator", "autoware::trajectory_validator::plugin::ValidatorInterface"),
   vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo())
 {
-  const auto filters = params_.filter_names;
+  concatenator_ptr_ =
+    std::make_unique<trajectory_concatenator::TrajectoryConcatenator>(params_.concatenator_params);
+  const auto filters = params_.validator_params.filter_names;
   for (const auto & filter : filters) {
     load_metric(filter);
   }
 
   constexpr bool shadow_mode = true;
-  for (const auto & filter : params_.shadow_mode_filter_names) {
+  for (const auto & filter : params_.validator_params.shadow_mode_filter_names) {
     load_metric(filter, shadow_mode);
   }
 
@@ -61,6 +62,10 @@ TrajectoryValidator::TrajectoryValidator(const rclcpp::NodeOptions & options)
 
   subscribers();
   publishers();
+
+  timer_ = create_timer(
+    this, get_clock(), std::chrono::milliseconds(30),
+    std::bind(&TrajectoryValidator::on_timer, this));
 }
 
 void TrajectoryValidator::subscribers()
@@ -69,9 +74,13 @@ void TrajectoryValidator::subscribers()
     "~/input/lanelet2_map", rclcpp::QoS{1}.transient_local(),
     std::bind(&TrajectoryValidator::map_callback, this, std::placeholders::_1));
 
-  sub_trajectories_ = create_subscription<CandidateTrajectories>(
-    "~/input/trajectories", 1,
-    std::bind(&TrajectoryValidator::process, this, std::placeholders::_1));
+  sub_trajectories_generative_ = create_subscription<CandidateTrajectories>(
+    "~/input/trajectories_generative", 1,
+    std::bind(&TrajectoryValidator::on_trajectories, this, std::placeholders::_1));
+
+  sub_trajectories_backup_ = create_subscription<CandidateTrajectories>(
+    "~/input/trajectories_backup", 1,
+    std::bind(&TrajectoryValidator::on_trajectories, this, std::placeholders::_1));
 }
 
 void TrajectoryValidator::publishers()
@@ -86,6 +95,13 @@ void TrajectoryValidator::publishers()
   pub_debug_ = std::make_shared<autoware_utils_debug::DebugPublisher>(this, "~/debug");
 }
 
+void TrajectoryValidator::on_trajectories(const CandidateTrajectories::ConstSharedPtr msg)
+{
+  // Passively update the buffer
+  std::lock_guard<std::mutex> lock(buffer_mutex_);
+  concatenator_ptr_->add_candidate(*msg);
+}
+
 tl::expected<FilterContext, std::string> TrajectoryValidator::take_data()
 {
   FilterContext context;
@@ -119,29 +135,40 @@ tl::expected<FilterContext, std::string> TrajectoryValidator::take_data()
   return context;
 }
 
-void TrajectoryValidator::process(const CandidateTrajectories::ConstSharedPtr msg)
+void TrajectoryValidator::on_timer()
 {
   autoware_utils_debug::ScopedTimeTrack st(__func__, *time_keeper_);
 
-  if (listener_.is_old(params_)) {
-    params_ = listener_.get_params();
-    for (const auto & plugin : plugins_) {
-      plugin->update_parameters(params_);
-    }
-    RCLCPP_INFO(get_logger(), "Dynamic parameters updated successfully.");
-  }
-
-  // 4. Instantiate and execute the stateless ValidationStage
-  ValidationStage validation_stage(plugins_);
-
+  // ==========================================
+  // 1. Data Synchronization & Parameters
+  // ==========================================
   auto context_opt = take_data();
   if (!context_opt) {
     RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 1000, "%s", context_opt.error().c_str());
     return;
   }
 
+  if (const auto is_params_updated = params_.update_parameters()) {
+    for (const auto & plugin : plugins_) {
+      plugin->update_parameters(params_.validator_params);
+    }
+    RCLCPP_INFO(get_logger(), "%s", is_params_updated->c_str());
+  }
+
+  // ==========================================
+  // 2. concatenated stage
+  // ==========================================
+  CandidateTrajectories concatenated_trajectories;
+  {
+    std::lock_guard<std::mutex> lock(buffer_mutex_);
+    concatenated_trajectories = concatenator_ptr_->get_concatenated(now());
+  }
+
+  diagnostics_interface_.clear();
+
   const auto & context = context_opt.value();
-  const auto report = validation_stage.process(*msg, context);
+  ValidationStage validation_stage(plugins_);
+  const auto report = validation_stage.process(concatenated_trajectories, context);
 
   diagnostics_interface_.clear();
 
@@ -160,7 +187,7 @@ void TrajectoryValidator::process(const CandidateTrajectories::ConstSharedPtr ms
 
   // 6. Publish outputs
   pub_trajectories_->publish(report.valid_trajectories);
-  update_diagnostic(*msg, report.num_feasible_trajectories);
+  update_diagnostic(concatenated_trajectories, report.num_feasible_trajectories);
 
   publish_validation_reports(report.validation_reports);
 
@@ -197,7 +224,7 @@ void TrajectoryValidator::load_metric(const std::string & name, const bool is_sh
 
     plugin->set_vehicle_info(vehicle_info_);
     plugin->set_shadow_mode(is_shadow_mode);
-    plugin->update_parameters(params_);
+    plugin->update_parameters(params_.validator_params);
     std::string category;
     size_t pos = name.find("::");
     if (pos != std::string::npos) {