feat(trajectory_validator): combine validator and concatenator

planning/autoware_trajectory_concatenator/CMakeLists.txt

@@ -13,24 +13,11 @@ include_directories(
 )
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/trajectory_concatenator_node.cpp
+  src/trajectory_concatenator.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}
   ${PROJECT_NAME}_param
 )
 
-target_compile_options(${PROJECT_NAME} PUBLIC
-  -Wno-error=deprecated-declarations
-)
-
-rclcpp_components_register_node(${PROJECT_NAME}
-  PLUGIN "autoware::trajectory_concatenator::TrajectoryConcatenatorNode"
-  EXECUTABLE ${PROJECT_NAME}_node
-)
-
-ament_auto_package(
-  INSTALL_TO_SHARE
-  config
-  launch
-)
+ament_auto_package()

planning/autoware_trajectory_concatenator/include/autoware/trajectory_concatenator/trajectory_concatenator.hpp

@@ -0,0 +1,61 @@
+// Copyright 2025 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_CONCATENATOR__TRAJECTORY_CONCATENATOR_HPP_
+#define AUTOWARE__TRAJECTORY_CONCATENATOR__TRAJECTORY_CONCATENATOR_HPP_
+
+#include <autoware_trajectory_concatenator/autoware_trajectory_concatenator_param.hpp>
+#include <builtin_interfaces/msg/time.hpp>
+
+#include <autoware_internal_planning_msgs/msg/candidate_trajectories.hpp>
+
+#include <string>
+#include <unordered_map>
+#include <utility>
+
+namespace autoware::trajectory_concatenator
+{
+
+using autoware_internal_planning_msgs::msg::CandidateTrajectories;
+
+/**
+ * @brief Stateful aggregator of trajectories from multiple generators.
+ * Uses a most-recent-per-generator buffer with time-based stale pruning.
+ * * * Pure C++ Library: This class is completely independent of rclcpp.
+ * * Thread-safety: Not thread-safe. Must be protected by a mutex by the caller.
+ * * ARCHITECTURAL NOTE ON LATENCY:
+ * Because this stage is designed to be flushed via a fixed-rate timer (e.g., 30ms),
+ * it introduces an average ~15ms (max 30ms) latency floor to the pipeline. Consequently,
+ * all downstream outputs (including debug markers) are quantized to this timer's rate.
+ */
+class TrajectoryConcatenator
+{
+public:
+  explicit TrajectoryConcatenator(concatenator::Params params) : params_(std::move(params)) {}
+
+  void update_parameters(const concatenator::Params & params) { params_ = params; }
+
+  void add_candidate(const CandidateTrajectories & msg);
+
+  [[nodiscard]] CandidateTrajectories get_concatenated(
+    const builtin_interfaces::msg::Time & current_time);
+
+private:
+  concatenator::Params params_;
+  std::unordered_map<std::string, CandidateTrajectories> buffer_;
+};
+
+}  // namespace autoware::trajectory_concatenator
+
+#endif  // AUTOWARE__TRAJECTORY_CONCATENATOR__TRAJECTORY_CONCATENATOR_HPP_

planning/autoware_trajectory_concatenator/src/trajectory_concatenator.cpp

@@ -0,0 +1,92 @@
+// Copyright 2025 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.
+
+#include "autoware/trajectory_concatenator/trajectory_concatenator.hpp"
+
+#include <autoware_utils_uuid/uuid_helper.hpp>
+
+#include <algorithm>
+#include <vector>
+
+namespace autoware::trajectory_concatenator
+{
+
+namespace
+{
+double to_seconds(const builtin_interfaces::msg::Time & time)
+{
+  return static_cast<double>(time.sec) + static_cast<double>(time.nanosec) * 1e-9;
+}
+}  // namespace
+
+void TrajectoryConcatenator::add_candidate(const CandidateTrajectories & msg)
+{
+  for (const auto & generator_info : msg.generator_info) {
+    const auto uuid = autoware_utils_uuid::to_hex_string(generator_info.generator_id);
+
+    auto trajectories = msg.candidate_trajectories;
+    const auto itr = std::remove_if(
+      trajectories.begin(), trajectories.end(),
+      [&generator_info](const auto & t) { return t.generator_id != generator_info.generator_id; });
+    trajectories.erase(itr, trajectories.end());
+
+    buffer_[uuid] = autoware_internal_planning_msgs::build<CandidateTrajectories>()
+                      .candidate_trajectories(trajectories)
+                      .generator_info({generator_info});
+  }
+}
+
+CandidateTrajectories TrajectoryConcatenator::get_concatenated(
+  const builtin_interfaces::msg::Time & current_time)
+{
+  std::vector<autoware_internal_planning_msgs::msg::CandidateTrajectory> trajectories;
+  std::vector<autoware_internal_planning_msgs::msg::GeneratorInfo> generator_info;
+
+  const double current_time_sec = to_seconds(current_time);
+
+  // Prune expired entries individually
+  for (auto it = buffer_.begin(); it != buffer_.end();) {
+    auto & pre_combine = it->second;
+
+    pre_combine.candidate_trajectories.erase(
+      std::remove_if(
+        pre_combine.candidate_trajectories.begin(), pre_combine.candidate_trajectories.end(),
+        [&](const auto & traj) {
+          const double msg_time_sec = to_seconds(traj.header.stamp);
+          return (current_time_sec - msg_time_sec) > params_.duration_time;
+        }),
+      pre_combine.candidate_trajectories.end());
+
+    if (pre_combine.candidate_trajectories.empty()) {
+      it = buffer_.erase(it);
+    } else {
+      it++;
+    }
+  }
+
+  // Combine surviving entries
+  for (const auto & [uuid, pre_combine] : buffer_) {
+    trajectories.insert(
+      trajectories.end(), pre_combine.candidate_trajectories.begin(),
+      pre_combine.candidate_trajectories.end());
+    generator_info.insert(
+      generator_info.end(), pre_combine.generator_info.begin(), pre_combine.generator_info.end());
+  }
+
+  return autoware_internal_planning_msgs::build<CandidateTrajectories>()
+    .candidate_trajectories(trajectories)
+    .generator_info(generator_info);
+}
+
+}  // namespace autoware::trajectory_concatenator