autowarefoundation
diff --git a/‎planning/autoware_trajectory_concatenator/CMakeLists.txt‎
Lines changed: 22 additions & 19 deletions b/‎planning/autoware_trajectory_concatenator/CMakeLists.txt‎
Lines changed: 22 additions & 19 deletions
diff --git a/‎planning/autoware_trajectory_concatenator/README.md‎
Lines changed: 6 additions & 5 deletions b/‎planning/autoware_trajectory_concatenator/README.md‎
Lines changed: 6 additions & 5 deletions
diff --git a/‎planning/autoware_trajectory_concatenator/config/concatenator.param.yaml‎
Lines changed: 0 additions & 3 deletions b/‎planning/autoware_trajectory_concatenator/config/concatenator.param.yaml‎
Lines changed: 0 additions & 3 deletions
diff --git a/‎planning/autoware_trajectory_concatenator/include/autoware/trajectory_concatenator/detail/trajectory_concatenator.hpp‎
Lines changed: 70 additions & 0 deletions b/‎planning/autoware_trajectory_concatenator/include/autoware/trajectory_concatenator/detail/trajectory_concatenator.hpp‎
Lines changed: 70 additions & 0 deletions
diff --git a/‎planning/autoware_trajectory_concatenator/include/autoware/trajectory_concatenator/trajectory_concatenator_wrapper.hpp‎
Lines changed: 96 additions & 0 deletions b/‎planning/autoware_trajectory_concatenator/include/autoware/trajectory_concatenator/trajectory_concatenator_wrapper.hpp‎
Lines changed: 96 additions & 0 deletions
diff --git a/‎planning/autoware_trajectory_concatenator/launch/trajectory_concatenator.launch.xml‎
Lines changed: 0 additions & 12 deletions b/‎planning/autoware_trajectory_concatenator/launch/trajectory_concatenator.launch.xml‎
Lines changed: 0 additions & 12 deletions
diff --git a/‎planning/autoware_trajectory_concatenator/package.xml‎
Lines changed: 1 addition & 0 deletions b/‎planning/autoware_trajectory_concatenator/package.xml‎
Lines changed: 1 addition & 0 deletions
diff --git a/‎planning/autoware_trajectory_concatenator/src/detail/trajectory_concatenator.cpp‎
Lines changed: 97 additions & 0 deletions b/‎planning/autoware_trajectory_concatenator/src/detail/trajectory_concatenator.cpp‎
Lines changed: 97 additions & 0 deletions
@@ -8,29 +8,32 @@ generate_parameter_library(${PROJECT_NAME}_param
   param/parameter_struct.yaml
 )
 
-include_directories(
-  SYSTEM
-)
-
 ament_auto_add_library(${PROJECT_NAME} SHARED
-  src/trajectory_concatenator_node.cpp
+  src/detail/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
-)
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  ament_auto_find_test_dependencies()
+
+  ament_add_gtest(test_trajectory_concatenator
+    test/test_trajectory_concatenator.cpp
+  )
+  target_link_libraries(test_trajectory_concatenator
+    ${PROJECT_NAME}
+  )
+
+  ament_add_gtest(test_trajectory_concatenator_wrapper
+    test/test_trajectory_concatenator_wrapper.cpp
+  )
+  target_link_libraries(test_trajectory_concatenator_wrapper
+    ${PROJECT_NAME}
+  )
+endif()
+
+ament_auto_package()
@@ -6,18 +6,19 @@ This node aggregates trajectory candidates from multiple trajectory generators i
 
 ## Algorithm Overview
 
-When a message arrives on `~/input/trajectories`, the node splits it by `generator_id` and updates an in-memory buffer so that only the most recent trajectory set for each generator is retained.
+When a `CandidateTrajectories` message is received (from either `~/input/trajectories_generative` or `~/input/trajectories_backup`), the library splits it by `generator_id` and updates an in-memory buffer so that only the most recent trajectory set for each generator is retained.
 
 A 100 ms timer then scans this buffer and drops any entry whose header stamp is older than the configured duration_time. Immediately after pruning, the timer concatenates all remaining trajectories and their accompanying `generator_info` arrays, publishes the aggregated message.
 
 ## Interface
 
 ### Topics
 
-| Direction  | Topic name              | Message Type                                                | Description                                    |
-| ---------- | ----------------------- | ----------------------------------------------------------- | ---------------------------------------------- |
-| Subscriber | `~/input/trajectories`  | `autoware_internal_planning_msgs/msg/CandidateTrajectories` | Trajectory sets produced by each generator     |
-| Publisher  | `~/output/trajectories` | `autoware_internal_planning_msgs/msg/CandidateTrajectories` | Concatenated list of all buffered trajectories |
+| Direction  | Topic name                        | Message Type                                                | Description                                    |
+| ---------- | --------------------------------- | ----------------------------------------------------------- | ---------------------------------------------- |
+| Subscriber | `~/input/trajectories_generative` | `autoware_internal_planning_msgs/msg/CandidateTrajectories` | Trajectory sets from generative planners       |
+| Subscriber | `~/input/trajectories_backup`     | `autoware_internal_planning_msgs/msg/CandidateTrajectories` | Trajectory sets from backup planners           |
+| Publisher  | `~/output/trajectories`           | `autoware_internal_planning_msgs/msg/CandidateTrajectories` | Concatenated list of all buffered trajectories |
 
 ### Parameters
 
 
@@ -0,0 +1,70 @@
+// 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__DETAIL__TRAJECTORY_CONCATENATOR_HPP_
+#define AUTOWARE__TRAJECTORY_CONCATENATOR__DETAIL__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 Aggregates candidate trajectories from multiple generators into a single set.
+ */
+class TrajectoryConcatenator
+{
+public:
+  /**
+   * @brief Constructs the concatenator with the given parameters.
+   * @param params Initial concatenator parameters.
+   */
+  explicit TrajectoryConcatenator(concatenator::Params params) : params_(std::move(params)) {}
+
+  /**
+   * @brief Replaces the current parameters.
+   * @param params New parameter values.
+   */
+  void update_parameters(const concatenator::Params & params) { params_ = params; }
+
+  /**
+   * @brief Stores the latest trajectories from the message's generator.
+   * @param msg Candidate trajectories message from a single generator.
+   */
+  void add_candidate(const CandidateTrajectories & msg);
+
+  /**
+   * @brief Returns the merged set of all non-stale generator trajectories.
+   * @param current_time Current ROS time used to filter out stale entries.
+   */
+  [[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__DETAIL__TRAJECTORY_CONCATENATOR_HPP_
@@ -0,0 +1,96 @@
+// 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_CONCATENATOR__TRAJECTORY_CONCATENATOR_WRAPPER_HPP_
+#define AUTOWARE__TRAJECTORY_CONCATENATOR__TRAJECTORY_CONCATENATOR_WRAPPER_HPP_
+
+#include <autoware/trajectory_concatenator/detail/trajectory_concatenator.hpp>
+#include <autoware_trajectory_concatenator/autoware_trajectory_concatenator_param.hpp>
+#include <builtin_interfaces/msg/time.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_internal_planning_msgs/msg/candidate_trajectories.hpp>
+
+#include <memory>
+#include <string>
+
+namespace autoware::trajectory_concatenator
+{
+/**
+ * @brief Adapter for TrajectoryConcatenator: handles parameter updates and thread safety.
+ */
+class TrajectoryConcatenatorWrapper
+{
+public:
+  /**
+   * @brief Constructs the wrapper and initialises the concatenator with declared parameters.
+   * @param node Node used for parameter declaration and logging.
+   * @param node_parameters_interface Parameter interface for declaring and reading parameters.
+   */
+  TrajectoryConcatenatorWrapper(
+    rclcpp::Node & node,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_interface)
+  : node_ptr_(&node),
+    logger_(node.get_logger().get_child(interface_name_)),
+    concatenator_params_listener_(node_parameters_interface),
+    concatenator_params_(concatenator_params_listener_.get_params()),
+    concatenator_ptr_(std::make_unique<TrajectoryConcatenator>(concatenator_params_))
+  {
+  }
+
+  /** @brief Reloads parameters from the parameter server if they have changed. */
+  void update_parameters()
+  {
+    std::lock_guard<std::mutex> lock(concatenator_mutex_);
+    if (concatenator_params_listener_.is_old(concatenator_params_)) {
+      concatenator_params_ = concatenator_params_listener_.get_params();
+      concatenator_ptr_->update_parameters(concatenator_params_);
+      RCLCPP_INFO(logger_, "Trajectory Concatenator parameters are updated.");
+    }
+  }
+
+  /**
+   * @brief Adds a set of candidate trajectories to the buffer.
+   * @param candidate_trajectories Candidate trajectories from a single generator.
+   */
+  void add_candidate(
+    const autoware_internal_planning_msgs::msg::CandidateTrajectories & candidate_trajectories)
+  {
+    std::lock_guard<std::mutex> lock(concatenator_mutex_);
+    concatenator_ptr_->add_candidate(candidate_trajectories);
+  }
+
+  /** @brief Returns the merged set of all non-stale generator trajectories. */
+  [[nodiscard]] autoware_internal_planning_msgs::msg::CandidateTrajectories get_concatenated()
+  {
+    update_parameters();
+    const auto time_now = node_ptr_->get_clock()->now();
+
+    std::lock_guard<std::mutex> lock(concatenator_mutex_);
+    return concatenator_ptr_->get_concatenated(time_now);
+  };
+
+private:
+  std::string interface_name_{"trajectory_concatenator"};
+  rclcpp::Node * node_ptr_{nullptr};
+  rclcpp::Logger logger_;
+  concatenator::ParamListener concatenator_params_listener_;
+  concatenator::Params concatenator_params_;
+  std::unique_ptr<trajectory_concatenator::TrajectoryConcatenator> concatenator_ptr_;
+  std::mutex concatenator_mutex_;
+};
+
+}  // namespace autoware::trajectory_concatenator
+
+#endif  // AUTOWARE__TRAJECTORY_CONCATENATOR__TRAJECTORY_CONCATENATOR_WRAPPER_HPP_
@@ -26,6 +26,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
 
+  <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 
 
@@ -0,0 +1,97 @@
+// 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/detail/trajectory_concatenator.hpp"
+
+#include <autoware_utils_uuid/uuid_helper.hpp>
+
+#include <algorithm>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#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
+using autoware_internal_planning_msgs::msg::CandidateTrajectory;
+
+void TrajectoryConcatenator::add_candidate(const CandidateTrajectories & msg)
+{
+  std::unordered_map<std::string, std::vector<CandidateTrajectory>> by_generator;
+  by_generator.reserve(msg.generator_info.size());
+  for (const auto & traj : msg.candidate_trajectories) {
+    by_generator[autoware_utils_uuid::to_hex_string(traj.generator_id)].push_back(traj);
+  }
+  for (const auto & generator_info : msg.generator_info) {
+    const auto uuid = autoware_utils_uuid::to_hex_string(generator_info.generator_id);
+    auto it = by_generator.find(uuid);
+    buffer_[uuid] =
+      autoware_internal_planning_msgs::build<CandidateTrajectories>()
+        .candidate_trajectories(
+          it != by_generator.end() ? std::move(it->second) : std::vector<CandidateTrajectory>{})
+        .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