Merge pull request #2668 from tier4/feat/e2e_cherry-pick_modules

feat(modifiler, safety_filter, traffic_rule_filter, ranker, adapeter): cherry pick some modules

Author: Kotakku

planning/autoware_trajectory_adapter/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_trajectory_adapter)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+include_directories(
+  SYSTEM
+)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/trajectory_adapter_node.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::trajectory_adapter::TrajectoryAdapterNode"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

planning/autoware_trajectory_adapter/README.md

@@ -0,0 +1,30 @@
+# Trajectory Adapter
+
+## Purpose/Role
+
+This node converts a set of ranked candidate trajectories into a single, cleaned‑up [autoware_planning_msgs/msg/Trajectory](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_planning_msgs/msg/Trajectory.msg).
+
+## Algorithm Overview
+
+Upon receiving a message, the node finds the candidate with the highest score.
+If no candidate is found an empty list would be published. The selected trajectory’s points are then processed, ensuring that successive points match the conditions to conduct control modules.
+
+## Interface
+
+### Topics
+
+| Direction | Topic Name             | Message Type                                                      | Description                        |
+| --------- | ---------------------- | ----------------------------------------------------------------- | ---------------------------------- |
+| Subscribe | `~/input/trajectories` | `autoware_internal_planning_msgs/msg/ScoredCandidateTrajectories` | Candidate trajectories with scores |
+| Publish   | `~/output/trajectory`  | `autoware_planning_msgs/msg/Trajectory`                           | Re-arranged best‑score trajectory  |
+
+### Parameters
+
+The current implementation does not expose any ROS parameters. All behavior is hard‑coded.
+
+## Future Work
+
+The following features and improvements are planned for future development:
+
+- **Turn Signal Control**: Implement proper turn signal activation based on trajectory curvature and lane change intentions
+- **Hazard Light Control**: Add logic to activate hazard lights in emergency situations or when the vehicle needs to warn other traffic participants

planning/autoware_trajectory_adapter/launch/trajectory_adapter.launch.xml

@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="input_trajectories" default="~/input/trajectories"/>
+  <arg name="output_trajectory" default="~/output/trajectory"/>
+
+  <node pkg="autoware_trajectory_adapter" exec="autoware_trajectory_adapter_node" name="trajectory_adapter_node" output="screen">
+    <remap from="~/input/trajectories" to="$(var input_trajectories)"/>
+    <remap from="~/output/trajectory" to="$(var output_trajectory)"/>
+  </node>
+</launch>

planning/autoware_trajectory_adapter/package.xml

@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_trajectory_adapter</name>
+  <version>0.1.0</version>
+  <description>The autoware_trajectory_adapter package</description>
+
+  <maintainer email="daniel.sanchez@tier4.jp">Daniel Sanchez</maintainer>
+  <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
+  <maintainer email="go.sakayori@tier4.jp">Go Sakayori</maintainer>
+  <maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="go.sakayori@tier4.jp">Go Sakayori</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_internal_planning_msgs</depend>
+  <depend>autoware_planning_msgs</depend>
+  <depend>autoware_utils_debug</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

planning/autoware_trajectory_adapter/src/trajectory_adapter_node.cpp

@@ -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.
+
+#include "trajectory_adapter_node.hpp"
+
+#include <memory>
+
+namespace autoware::trajectory_adapter
+{
+
+TrajectoryAdapterNode::TrajectoryAdapterNode(const rclcpp::NodeOptions & node_options)
+: Node{"trajectory_adapter_node", node_options},
+  sub_trajectories_{this->create_subscription<ScoredCandidateTrajectories>(
+    "~/input/trajectories", 1,
+    std::bind(&TrajectoryAdapterNode::process, this, std::placeholders::_1))},
+  pub_trajectory_{this->create_publisher<Trajectory>("~/output/trajectory", 1)}
+{
+  debug_processing_time_detail_pub_ = create_publisher<autoware_utils_debug::ProcessingTimeDetail>(
+    "~/debug/processing_time_detail_ms/trajectory_adapter", 1);
+  time_keeper_ =
+    std::make_shared<autoware_utils_debug::TimeKeeper>(debug_processing_time_detail_pub_);
+}
+
+void TrajectoryAdapterNode::process(const ScoredCandidateTrajectories::ConstSharedPtr msg)
+{
+  autoware_utils_debug::ScopedTimeTrack st(__func__, *time_keeper_);
+
+  if (msg->scored_candidate_trajectories.empty()) {
+    return;
+  }
+
+  const auto trajectory_itr = std::max_element(
+    msg->scored_candidate_trajectories.begin(), msg->scored_candidate_trajectories.end(),
+    [](const auto & a, const auto & b) { return a.score < b.score; });
+
+  const auto best_generator = [&msg](const auto & uuid) {
+    const auto generator_itr = std::find_if(
+      msg->generator_info.begin(), msg->generator_info.end(),
+      [&uuid](const auto & info) { return info.generator_id == uuid; });
+    return generator_itr == msg->generator_info.end() ? "NOT FOUND"
+                                                      : generator_itr->generator_name.data;
+  };
+
+  RCLCPP_DEBUG_STREAM(
+    this->get_logger(),
+    "best generator:" << best_generator(trajectory_itr->candidate_trajectory.generator_id)
+                      << " score:" << trajectory_itr->score);
+
+  const auto trajectory = autoware_planning_msgs::build<Trajectory>()
+                            .header(trajectory_itr->candidate_trajectory.header)
+                            .points(trajectory_itr->candidate_trajectory.points);
+
+  pub_trajectory_->publish(trajectory);
+}
+
+}  // namespace autoware::trajectory_adapter
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::trajectory_adapter::TrajectoryAdapterNode)

planning/autoware_trajectory_adapter/src/trajectory_adapter_node.hpp

@@ -0,0 +1,51 @@
+// 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 TRAJECTORY_ADAPTER_NODE_HPP_
+#define TRAJECTORY_ADAPTER_NODE_HPP_
+
+#include <autoware_utils_debug/time_keeper.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_internal_planning_msgs/msg/scored_candidate_trajectories.hpp>
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+
+#include <memory>
+
+namespace autoware::trajectory_adapter
+{
+
+using autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories;
+using autoware_planning_msgs::msg::Trajectory;
+
+class TrajectoryAdapterNode : public rclcpp::Node
+{
+public:
+  explicit TrajectoryAdapterNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void process(const ScoredCandidateTrajectories::ConstSharedPtr msg);
+
+  rclcpp::Subscription<ScoredCandidateTrajectories>::SharedPtr sub_trajectories_;
+
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_;
+
+  rclcpp::Publisher<autoware_utils_debug::ProcessingTimeDetail>::SharedPtr
+    debug_processing_time_detail_pub_;
+  mutable std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper_{nullptr};
+};
+
+}  // namespace autoware::trajectory_adapter
+
+#endif  // TRAJECTORY_ADAPTER_NODE_HPP_

planning/autoware_trajectory_concatenator/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_trajectory_concatenator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+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
+)
+
+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
+)

planning/autoware_trajectory_concatenator/README.md

@@ -0,0 +1,24 @@
+# Trajectory Concatenator
+
+## Purpose/Role
+
+This node aggregates trajectory candidates from multiple trajectory generators into a single `autoware_internal_planning_msgs/msg/CandidateTrajectories` message. It is intended to be placed between trajectory generators and the selector/ranker.
+
+## 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.
+
+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 |
+
+### Parameters
+
+{{ json_to_markdown("planning/autoware_trajectory_concatenator/schema/trajectory_concatenator.schema.json") }}

planning/autoware_trajectory_concatenator/config/concatenator.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    duration_time: 0.2 #[s]

planning/autoware_trajectory_concatenator/launch/trajectory_concatenator.launch.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="concatenator_input_trajectories" default="/planning/candidate/trajectories"/>
+  <arg name="concatenator_output_trajectories" default="/planning/trajectory_selector/concatenate/trajectories"/>
+  <arg name="trajectory_concatenator_param_path" default="$(find-pkg-share autoware_trajectory_concatenator)/config/concatenator.param.yaml"/>
+
+  <node pkg="autoware_trajectory_concatenator" exec="autoware_trajectory_concatenator_node" name="trajectory_concatenator_node" output="screen">
+    <param from="$(var trajectory_concatenator_param_path)" allow_substs="true"/>
+    <remap from="~/input/trajectories" to="$(var concatenator_input_trajectories)"/>
+    <remap from="~/output/trajectories" to="$(var concatenator_output_trajectories)"/>
+  </node>
+</launch>