Skip to content

Commit 2d0b4bc

Browse files
go-sakayoriKotakku
go-sakayori
authored and
Kotakku
committed
feat(trajectory_adapter): add trajectory adapter (autowarefoundation#11324)
* add trajectory adaptor Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * change spelling from adaptor to adapter Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix spelling Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * disable spelling check for commnet Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * fix header include files Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * remove function removeOverlapPoints Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * remove motion_utils Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * remove end() check Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> * add node suffix Signed-off-by: Go Sakayori <go.sakayori@tier4.jp> --------- Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>
1 parent 31bb070 commit 2d0b4bc

File tree

6 files changed

+216
-0
lines changed

6 files changed

+216
-0
lines changed

‎planning/autoware_trajectory_adapter/CMakeLists.txt‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
cmake_minimum_required(VERSION 3.14)
2+
project(autoware_trajectory_adapter)
3+
4+
find_package(autoware_cmake REQUIRED)
5+
autoware_package()
6+
7+
include_directories(
8+
SYSTEM
9+
)
10+
11+
ament_auto_add_library(${PROJECT_NAME} SHARED
12+
src/trajectory_adapter_node.cpp
13+
)
14+
15+
rclcpp_components_register_node(${PROJECT_NAME}
16+
PLUGIN "autoware::trajectory_adapter::TrajectoryAdapterNode"
17+
EXECUTABLE ${PROJECT_NAME}_node
18+
)
19+
20+
ament_auto_package(
21+
INSTALL_TO_SHARE
22+
launch
23+
)

‎planning/autoware_trajectory_adapter/README.md‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
# Trajectory Adapter
2+
3+
## Purpose/Role
4+
5+
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).
6+
7+
## Algorithm Overview
8+
9+
Upon receiving a message, the node finds the candidate with the highest score.
10+
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.
11+
12+
## Interface
13+
14+
### Topics
15+
16+
| Direction | Topic Name | Message Type | Description |
17+
| --------- | ---------------------- | ----------------------------------------------------------------- | ---------------------------------- |
18+
| Subscribe | `~/input/trajectories` | `autoware_internal_planning_msgs/msg/ScoredCandidateTrajectories` | Candidate trajectories with scores |
19+
| Publish | `~/output/trajectory` | `autoware_planning_msgs/msg/Trajectory` | Re-arranged best‑score trajectory |
20+
21+
### Parameters
22+
23+
The current implementation does not expose any ROS parameters. All behavior is hard‑coded.
24+
25+
## Future Work
26+
27+
The following features and improvements are planned for future development:
28+
29+
- **Turn Signal Control**: Implement proper turn signal activation based on trajectory curvature and lane change intentions
30+
- **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‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
<?xml version="1.0" encoding="UTF-8"?>
2+
<launch>
3+
<arg name="input_trajectories" default="~/input/trajectories"/>
4+
<arg name="output_trajectory" default="~/output/trajectory"/>
5+
6+
<node pkg="autoware_trajectory_adapter" exec="autoware_trajectory_adapter_node" name="trajectory_adapter_node" output="screen">
7+
<remap from="~/input/trajectories" to="$(var input_trajectories)"/>
8+
<remap from="~/output/trajectory" to="$(var output_trajectory)"/>
9+
</node>
10+
</launch>

‎planning/autoware_trajectory_adapter/package.xml‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>autoware_trajectory_adapter</name>
5+
<version>0.1.0</version>
6+
<description>The autoware_trajectory_adapter package</description>
7+
8+
<maintainer email="daniel.sanchez@tier4.jp">Daniel Sanchez</maintainer>
9+
<maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
10+
<maintainer email="go.sakayori@tier4.jp">Go Sakayori</maintainer>
11+
<maintainer email="shintaro.sakoda@tier4.jp">Shintaro Sakoda</maintainer>
12+
13+
<license>Apache License 2.0</license>
14+
15+
<author email="go.sakayori@tier4.jp">Go Sakayori</author>
16+
17+
<buildtool_depend>ament_cmake</buildtool_depend>
18+
<buildtool_depend>autoware_cmake</buildtool_depend>
19+
20+
<depend>autoware_internal_planning_msgs</depend>
21+
<depend>autoware_planning_msgs</depend>
22+
<depend>autoware_utils_debug</depend>
23+
<depend>rclcpp</depend>
24+
<depend>rclcpp_components</depend>
25+
26+
<test_depend>ament_lint_auto</test_depend>
27+
<test_depend>autoware_lint_common</test_depend>
28+
29+
<export>
30+
<build_type>ament_cmake</build_type>
31+
</export>
32+
</package>

‎planning/autoware_trajectory_adapter/src/trajectory_adapter_node.cpp‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "trajectory_adapter_node.hpp"
16+
17+
#include <memory>
18+
19+
namespace autoware::trajectory_adapter
20+
{
21+
22+
TrajectoryAdapterNode::TrajectoryAdapterNode(const rclcpp::NodeOptions & node_options)
23+
: Node{"trajectory_adapter_node", node_options},
24+
sub_trajectories_{this->create_subscription<ScoredCandidateTrajectories>(
25+
"~/input/trajectories", 1,
26+
std::bind(&TrajectoryAdapterNode::process, this, std::placeholders::_1))},
27+
pub_trajectory_{this->create_publisher<Trajectory>("~/output/trajectory", 1)}
28+
{
29+
debug_processing_time_detail_pub_ = create_publisher<autoware_utils_debug::ProcessingTimeDetail>(
30+
"~/debug/processing_time_detail_ms/trajectory_adapter", 1);
31+
time_keeper_ =
32+
std::make_shared<autoware_utils_debug::TimeKeeper>(debug_processing_time_detail_pub_);
33+
}
34+
35+
void TrajectoryAdapterNode::process(const ScoredCandidateTrajectories::ConstSharedPtr msg)
36+
{
37+
autoware_utils_debug::ScopedTimeTrack st(__func__, *time_keeper_);
38+
39+
if (msg->scored_candidate_trajectories.empty()) {
40+
return;
41+
}
42+
43+
const auto trajectory_itr = std::max_element(
44+
msg->scored_candidate_trajectories.begin(), msg->scored_candidate_trajectories.end(),
45+
[](const auto & a, const auto & b) { return a.score < b.score; });
46+
47+
const auto best_generator = [&msg](const auto & uuid) {
48+
const auto generator_itr = std::find_if(
49+
msg->generator_info.begin(), msg->generator_info.end(),
50+
[&uuid](const auto & info) { return info.generator_id == uuid; });
51+
return generator_itr == msg->generator_info.end() ? "NOT FOUND"
52+
: generator_itr->generator_name.data;
53+
};
54+
55+
RCLCPP_DEBUG_STREAM(
56+
this->get_logger(),
57+
"best generator:" << best_generator(trajectory_itr->candidate_trajectory.generator_id)
58+
<< " score:" << trajectory_itr->score);
59+
60+
const auto trajectory = autoware_planning_msgs::build<Trajectory>()
61+
.header(trajectory_itr->candidate_trajectory.header)
62+
.points(trajectory_itr->candidate_trajectory.points);
63+
64+
pub_trajectory_->publish(trajectory);
65+
}
66+
67+
} // namespace autoware::trajectory_adapter
68+
69+
#include <rclcpp_components/register_node_macro.hpp>
70+
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::trajectory_adapter::TrajectoryAdapterNode)

‎planning/autoware_trajectory_adapter/src/trajectory_adapter_node.hpp‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
// Copyright 2025 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef TRAJECTORY_ADAPTER_NODE_HPP_
16+
#define TRAJECTORY_ADAPTER_NODE_HPP_
17+
18+
#include <autoware_utils_debug/time_keeper.hpp>
19+
#include <rclcpp/rclcpp.hpp>
20+
21+
#include <autoware_internal_planning_msgs/msg/scored_candidate_trajectories.hpp>
22+
#include <autoware_planning_msgs/msg/trajectory.hpp>
23+
24+
#include <memory>
25+
26+
namespace autoware::trajectory_adapter
27+
{
28+
29+
using autoware_internal_planning_msgs::msg::ScoredCandidateTrajectories;
30+
using autoware_planning_msgs::msg::Trajectory;
31+
32+
class TrajectoryAdapterNode : public rclcpp::Node
33+
{
34+
public:
35+
explicit TrajectoryAdapterNode(const rclcpp::NodeOptions & node_options);
36+
37+
private:
38+
void process(const ScoredCandidateTrajectories::ConstSharedPtr msg);
39+
40+
rclcpp::Subscription<ScoredCandidateTrajectories>::SharedPtr sub_trajectories_;
41+
42+
rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_;
43+
44+
rclcpp::Publisher<autoware_utils_debug::ProcessingTimeDetail>::SharedPtr
45+
debug_processing_time_detail_pub_;
46+
mutable std::shared_ptr<autoware_utils_debug::TimeKeeper> time_keeper_{nullptr};
47+
};
48+
49+
} // namespace autoware::trajectory_adapter
50+
51+
#endif // TRAJECTORY_ADAPTER_NODE_HPP_

0 commit comments

Comments
 (0)