diff --git a/control/autoware_simple_pure_pursuit/CMakeLists.txt b/control/autoware_simple_pure_pursuit/CMakeLists.txt
new file mode 100644
index 0000000000..72f8f60287
--- /dev/null
+++ b/control/autoware_simple_pure_pursuit/CMakeLists.txt
@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(autoware_simple_pure_pursuit)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME}_lib SHARED
+ DIRECTORY src
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}_lib
+ PLUGIN "autoware::control::simple_pure_pursuit::SimplePurePursuitNode"
+ EXECUTABLE ${PROJECT_NAME}_exe
+)
+
+if(BUILD_TESTING)
+ ament_add_ros_isolated_gtest(${PROJECT_NAME}_test
+ test/test_simple_pure_pursuit.cpp
+ )
+ target_link_libraries(${PROJECT_NAME}_test
+ ${PROJECT_NAME}_lib
+ )
+endif()
+
+ament_auto_package(
+ INSTALL_TO_SHARE
+ launch
+ config
+)
diff --git a/control/autoware_simple_pure_pursuit/README.md b/control/autoware_simple_pure_pursuit/README.md
new file mode 100644
index 0000000000..638af20028
--- /dev/null
+++ b/control/autoware_simple_pure_pursuit/README.md
@@ -0,0 +1,24 @@
+# Simple Pure Pursuit
+
+The `simple_pure_pursuit` node receives a reference trajectory from `motion_velocity_smoother` and calculates the control command using the pure pursuit algorithm.
+
+## Flowchart
+
+
+
+## Input topics
+
+| Name | Type | Description |
+| :------------------- | :---------------------------------------- | :------------------- |
+| `~/input/odometry` | `nav_msgs::msg::Odometry` | ego odometry |
+| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | reference trajectory |
+
+## Output topics
+
+| Name | Type | Description | QoS Durability |
+| :------------------------- | :------------------------------------ | :-------------- | :------------- |
+| `~/output/control_command` | `autoware_control_msgs::msg::Control` | control command | `volatile` |
+
+## Parameters
+
+{{ json_to_markdown("control/autoware_simple_pure_pursuit/schema/simple_pure_pursuit.schema.json") }}
diff --git a/control/autoware_simple_pure_pursuit/config/simple_pure_pursuit.param.yaml b/control/autoware_simple_pure_pursuit/config/simple_pure_pursuit.param.yaml
new file mode 100644
index 0000000000..f78c863901
--- /dev/null
+++ b/control/autoware_simple_pure_pursuit/config/simple_pure_pursuit.param.yaml
@@ -0,0 +1,7 @@
+/**:
+ ros__parameters:
+ lookahead_gain: 1.0
+ lookahead_min_distance: 1.0
+ speed_proportional_gain: 1.0
+ use_external_target_vel: false
+ external_target_vel: 1.0
diff --git a/control/autoware_simple_pure_pursuit/launch/simple_pure_pursuit.launch.xml b/control/autoware_simple_pure_pursuit/launch/simple_pure_pursuit.launch.xml
new file mode 100644
index 0000000000..7bd6e7e0db
--- /dev/null
+++ b/control/autoware_simple_pure_pursuit/launch/simple_pure_pursuit.launch.xml
@@ -0,0 +1,12 @@
+<launch>
+ <arg name="vehicle_info_param_file" default="$(find-pkg-share autoware_vehicle_info_utils)/config/vehicle_info.param.yaml"/>
+
+ <node pkg="autoware_simple_pure_pursuit" exec="autoware_simple_pure_pursuit_exe" name="simple_pure_pursuit" output="screen">
+ <param from="$(find-pkg-share autoware_simple_pure_pursuit)/config/simple_pure_pursuit.param.yaml"/>
+ <param from="$(var vehicle_info_param_file)"/>
+
+ <remap from="~/input/odometry" to="/localization/kinematic_state"/>
+ <remap from="~/input/trajectory" to="/planning/scenario_planning/trajectory"/>
+ <remap from="~/output/control_command" to="/control/trajectory_follower/control_cmd"/>
+ </node>
+</launch>
diff --git a/control/autoware_simple_pure_pursuit/package.xml b/control/autoware_simple_pure_pursuit/package.xml
new file mode 100644
index 0000000000..7d19b72ed0
--- /dev/null
+++ b/control/autoware_simple_pure_pursuit/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_simple_pure_pursuit</name>
+ <version>0.0.0</version>
+ <description>The autoware_simple_pure_pursuit package</description>
+ <maintainer email="yuki.takagi@tier4.jp">Yuki Takagi</maintainer>
+ <maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
+ <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
+
+ <license>Apache License 2.0</license>
+
+ <buildtool_depend>ament_cmake_auto</buildtool_depend>
+ <buildtool_depend>autoware_cmake</buildtool_depend>
+
+ <depend>autoware_control_msgs</depend>
+ <depend>autoware_motion_utils</depend>
+ <depend>autoware_planning_msgs</depend>
+ <depend>autoware_test_utils</depend>
+ <depend>autoware_utils</depend>
+ <depend>autoware_vehicle_info_utils</depend>
+ <depend>rclcpp</depend>
+ <depend>rclcpp_components</depend>
+
+ <test_depend>ament_cmake_ros</test_depend>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>autoware_lint_common</test_depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>
diff --git a/control/autoware_simple_pure_pursuit/schema/simple_pure_pursuit.schema.json b/control/autoware_simple_pure_pursuit/schema/simple_pure_pursuit.schema.json
new file mode 100644
index 0000000000..76b353fa01
--- /dev/null
+++ b/control/autoware_simple_pure_pursuit/schema/simple_pure_pursuit.schema.json
@@ -0,0 +1,44 @@
+{
+ "$schema": "http://json-schema.org/draft-07/schema#",
+ "title": "Parameters for Simple Pure Pursuit Node",
+ "type": "object",
+ "definitions": {
+ "autoware_simple_pure_pursuit": {
+ "type": "object",
+ "properties": {
+ "lookahead_gain": {
+ "type": "number",
+ "description": "Gain for lookahead distance calculation: {lookahead distance} = lookahead_gain * {target velocity} + lookahead_min_distance",
+ "minimum": 0.0
+ },
+ "lookahead_min_distance": {
+ "type": "number",
+ "description": "Minimum lookahead distance [m]",
+ "minimum": 0.0
+ },
+ "speed_proportional_gain": {
+ "type": "number",
+ "description": "Gain for longitudinal acceleration calculation: {longitudinal acceleration} = speed_proportional_gain * ({target velocity} - {current velocity})",
+ "minimum": 0.0
+ },
+ "use_external_target_vel": {
+ "type": "boolean",
+ "description": "Whether to use external target velocity"
+ },
+ "external_target_vel": {
+ "type": "number",
+ "description": "External target velocity [m/s]",
+ "minimum": 0.0
+ }
+ },
+ "required": [
+ "lookahead_gain",
+ "lookahead_min_distance",
+ "speed_proportional_gain",
+ "use_external_target_vel",
+ "external_target_vel"
+ ],
+ "additionalProperties": false
+ }
+ }
+}
diff --git a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp
new file mode 100644
index 0000000000..2a25617cb1
--- /dev/null
+++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp
@@ -0,0 +1,145 @@
+// Copyright 2024 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 "simple_pure_pursuit.hpp"
+
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware_utils/geometry/pose_deviation.hpp>
+
+#include <tf2/utils.h>
+
+#include <algorithm>
+
+namespace autoware::control::simple_pure_pursuit
+{
+using autoware::motion_utils::findNearestIndex;
+
+SimplePurePursuitNode::SimplePurePursuitNode(const rclcpp::NodeOptions & node_options)
+: Node("simple_pure_pursuit", node_options),
+ pub_control_command_(
+ create_publisher<autoware_control_msgs::msg::Control>("~/output/control_command", 1)),
+ vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()),
+ lookahead_gain_(declare_parameter<float>("lookahead_gain")),
+ lookahead_min_distance_(declare_parameter<float>("lookahead_min_distance")),
+ speed_proportional_gain_(declare_parameter<float>("speed_proportional_gain")),
+ use_external_target_vel_(declare_parameter<bool>("use_external_target_vel")),
+ external_target_vel_(declare_parameter<float>("external_target_vel"))
+{
+ using namespace std::literals::chrono_literals;
+ timer_ = rclcpp::create_timer(
+ this, get_clock(), 30ms, std::bind(&SimplePurePursuitNode::on_timer, this));
+}
+
+void SimplePurePursuitNode::on_timer()
+{
+ // 1. subscribe data
+ const auto odom_ptr = odom_sub_.take_data();
+ const auto traj_ptr = traj_sub_.take_data();
+ if (!odom_ptr || !traj_ptr) {
+ return;
+ }
+
+ // 2. extract subscribed data
+ const auto odom = *odom_ptr;
+ const auto traj = *traj_ptr;
+
+ // 3. create control command
+ const auto control_command = create_control_command(odom, traj);
+
+ // 4. publish control command
+ pub_control_command_->publish(control_command);
+}
+
+autoware_control_msgs::msg::Control SimplePurePursuitNode::create_control_command(
+ const Odometry & odom, const Trajectory & traj)
+{
+ const size_t closest_traj_point_idx = findNearestIndex(traj.points, odom.pose.pose.position);
+
+ // when the ego reaches the goal
+ if (closest_traj_point_idx == traj.points.size() - 1 || traj.points.size() <= 5) {
+ autoware_control_msgs::msg::Control control_command;
+ control_command.stamp = get_clock()->now();
+ control_command.longitudinal.velocity = 0.0;
+ control_command.longitudinal.acceleration = -10.0;
+ RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 5000, "reached to the goal");
+ return control_command;
+ }
+
+ // calculate target longitudinal velocity
+ const double target_longitudinal_vel =
+ use_external_target_vel_ ? external_target_vel_
+ : traj.points.at(closest_traj_point_idx).longitudinal_velocity_mps;
+
+ // calculate control command
+ autoware_control_msgs::msg::Control control_command;
+ control_command.longitudinal = calc_longitudinal_control(odom, target_longitudinal_vel);
+ control_command.lateral =
+ calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx);
+
+ return control_command;
+}
+
+autoware_control_msgs::msg::Longitudinal SimplePurePursuitNode::calc_longitudinal_control(
+ const Odometry & odom, const double target_longitudinal_vel) const
+{
+ const double current_longitudinal_vel = odom.twist.twist.linear.x;
+
+ autoware_control_msgs::msg::Longitudinal longitudinal_control_command;
+ longitudinal_control_command.velocity = target_longitudinal_vel;
+ longitudinal_control_command.acceleration =
+ speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel);
+
+ return longitudinal_control_command;
+}
+
+autoware_control_msgs::msg::Lateral SimplePurePursuitNode::calc_lateral_control(
+ const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel,
+ const size_t closest_traj_point_idx) const
+{
+ // calculate lookahead distance
+ const double lookahead_distance =
+ lookahead_gain_ * target_longitudinal_vel + lookahead_min_distance_;
+
+ // calculate center coordinate of rear wheel
+ const double rear_x = odom.pose.pose.position.x -
+ vehicle_info_.wheel_base_m / 2.0 * std::cos(odom.pose.pose.orientation.z);
+ const double rear_y = odom.pose.pose.position.y -
+ vehicle_info_.wheel_base_m / 2.0 * std::sin(odom.pose.pose.orientation.z);
+
+ // search lookahead point
+ auto lookahead_point_itr = std::find_if(
+ traj.points.begin() + closest_traj_point_idx, traj.points.end(),
+ [&](const TrajectoryPoint & point) {
+ return std::hypot(point.pose.position.x - rear_x, point.pose.position.y - rear_y) >=
+ lookahead_distance;
+ });
+ if (lookahead_point_itr == traj.points.end()) {
+ lookahead_point_itr = traj.points.end() - 1;
+ }
+ const double lookahead_point_x = lookahead_point_itr->pose.position.x;
+ const double lookahead_point_y = lookahead_point_itr->pose.position.y;
+
+ // calculate steering angle
+ autoware_control_msgs::msg::Lateral lateral_control_command;
+ const double alpha = std::atan2(lookahead_point_y - rear_y, lookahead_point_x - rear_x) -
+ tf2::getYaw(odom.pose.pose.orientation);
+ lateral_control_command.steering_tire_angle =
+ std::atan2(2.0 * vehicle_info_.wheel_base_m * std::sin(alpha), lookahead_distance);
+
+ return lateral_control_command;
+}
+} // namespace autoware::control::simple_pure_pursuit
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::control::simple_pure_pursuit::SimplePurePursuitNode)
diff --git a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp
new file mode 100644
index 0000000000..09d86a7f35
--- /dev/null
+++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp
@@ -0,0 +1,75 @@
+// Copyright 2024 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 SIMPLE_PURE_PURSUIT_HPP_
+#define SIMPLE_PURE_PURSUIT_HPP_
+
+#include <autoware_utils/ros/polling_subscriber.hpp>
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_control_msgs/msg/control.hpp>
+#include <autoware_planning_msgs/msg/trajectory.hpp>
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+namespace autoware::control::simple_pure_pursuit
+{
+using autoware_planning_msgs::msg::Trajectory;
+using autoware_planning_msgs::msg::TrajectoryPoint;
+using nav_msgs::msg::Odometry;
+
+class SimplePurePursuitNode : public rclcpp::Node
+{
+public:
+ explicit SimplePurePursuitNode(const rclcpp::NodeOptions & node_options);
+
+private:
+ // subscribers
+ autoware_utils::InterProcessPollingSubscriber<Odometry> odom_sub_{this, "~/input/odometry"};
+ autoware_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{this, "~/input/trajectory"};
+
+ // publishers
+ rclcpp::Publisher<autoware_control_msgs::msg::Control>::SharedPtr pub_control_command_;
+
+ // timer
+ rclcpp::TimerBase::SharedPtr timer_;
+
+ // vehicle info
+ const autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
+
+ // pure pursuit parameters
+ const double lookahead_gain_;
+ const double lookahead_min_distance_;
+ const double speed_proportional_gain_;
+ const bool use_external_target_vel_;
+ const double external_target_vel_;
+
+ // functions
+ void on_timer();
+ autoware_control_msgs::msg::Control create_control_command(
+ const Odometry & odom, const Trajectory & traj);
+ autoware_control_msgs::msg::Longitudinal calc_longitudinal_control(
+ const Odometry & odom, const double target_longitudinal_vel) const;
+ autoware_control_msgs::msg::Lateral calc_lateral_control(
+ const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel,
+ const size_t closest_traj_point_idx) const;
+
+public:
+ friend class SimplePurePursuitNodeTest;
+};
+
+} // namespace autoware::control::simple_pure_pursuit
+
+#endif // SIMPLE_PURE_PURSUIT_HPP_
diff --git a/control/autoware_simple_pure_pursuit/test/test_simple_pure_pursuit.cpp b/control/autoware_simple_pure_pursuit/test/test_simple_pure_pursuit.cpp
new file mode 100644
index 0000000000..36062eb0be
--- /dev/null
+++ b/control/autoware_simple_pure_pursuit/test/test_simple_pure_pursuit.cpp
@@ -0,0 +1,155 @@
+// Copyright 2024 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 "../src/simple_pure_pursuit.hpp"
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <autoware_test_utils/autoware_test_utils.hpp>
+
+#include <gtest/gtest.h>
+
+#include <memory>
+
+namespace autoware::control::simple_pure_pursuit
+{
+Odometry makeOdometry(const double x, const double y, const double yaw)
+{
+ Odometry odom;
+ odom.pose.pose.position.x = x;
+ odom.pose.pose.position.y = y;
+ odom.pose.pose.orientation.z = std::sin(yaw / 2);
+ odom.pose.pose.orientation.w = std::cos(yaw / 2);
+ return odom;
+}
+
+class SimplePurePursuitNodeTest : public ::testing::Test
+{
+protected:
+ void SetUp() override
+ {
+ rclcpp::init(0, nullptr);
+
+ const auto autoware_test_utils_dir =
+ ament_index_cpp::get_package_share_directory("autoware_test_utils");
+ const auto autoware_simple_pure_pursuit_dir =
+ ament_index_cpp::get_package_share_directory("autoware_simple_pure_pursuit");
+
+ auto node_options = rclcpp::NodeOptions{};
+ autoware::test_utils::updateNodeOptions(
+ node_options, {autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml",
+ autoware_simple_pure_pursuit_dir + "/config/simple_pure_pursuit.param.yaml"});
+
+ node_ = std::make_shared<SimplePurePursuitNode>(node_options);
+ }
+
+ void TearDown() override { rclcpp::shutdown(); }
+
+ autoware_control_msgs::msg::Control create_control_command(
+ const Odometry & odom, const Trajectory & traj) const
+ {
+ return node_->create_control_command(odom, traj);
+ }
+
+ autoware_control_msgs::msg::Longitudinal calc_longitudinal_control(
+ const Odometry & odom, const double target_longitudinal_vel) const
+ {
+ return node_->calc_longitudinal_control(odom, target_longitudinal_vel);
+ }
+
+ autoware_control_msgs::msg::Lateral calc_lateral_control(
+ const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel,
+ const size_t closest_traj_point_idx) const
+ {
+ return node_->calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx);
+ }
+
+ double speed_proportional_gain() const { return node_->speed_proportional_gain_; }
+
+private:
+ std::shared_ptr<SimplePurePursuitNode> node_;
+};
+
+TEST_F(SimplePurePursuitNodeTest, create_control_command)
+{
+ { // normal case
+ const auto odom = makeOdometry(0.0, 0.0, 0.0);
+ const auto traj = autoware::test_utils::generateTrajectory<Trajectory>(10, 1.0, 1.0);
+
+ const auto result = create_control_command(odom, traj);
+
+ EXPECT_DOUBLE_EQ(result.longitudinal.velocity, 1.0);
+ EXPECT_DOUBLE_EQ(result.longitudinal.acceleration, speed_proportional_gain() * 1.0);
+ }
+
+ { // ego reached goal
+ const auto odom = makeOdometry(10.0, 0.0, 0.0);
+ const auto traj = autoware::test_utils::generateTrajectory<Trajectory>(10, 1.0, 1.0);
+
+ const auto result = create_control_command(odom, traj);
+
+ EXPECT_DOUBLE_EQ(result.longitudinal.velocity, 0.0);
+ EXPECT_DOUBLE_EQ(result.longitudinal.acceleration, -10.0);
+ }
+
+ { // reference trajectory is too short
+ const auto odom = makeOdometry(0.0, 0.0, 0.0);
+ const auto traj = autoware::test_utils::generateTrajectory<Trajectory>(5, 1.0, 1.0);
+
+ const auto result = create_control_command(odom, traj);
+
+ EXPECT_DOUBLE_EQ(result.longitudinal.velocity, 0.0);
+ EXPECT_DOUBLE_EQ(result.longitudinal.acceleration, -10.0);
+ }
+}
+
+TEST_F(SimplePurePursuitNodeTest, calc_longitudinal_control)
+{
+ { // normal case
+ const auto odom = makeOdometry(0.0, 0.0, 0.0);
+ const auto target_longitudinal_vel = 1.0;
+
+ const auto result = calc_longitudinal_control(odom, target_longitudinal_vel);
+
+ EXPECT_DOUBLE_EQ(result.velocity, 1.0);
+ EXPECT_DOUBLE_EQ(result.acceleration, speed_proportional_gain() * 1.0);
+ }
+}
+
+TEST_F(SimplePurePursuitNodeTest, calc_lateral_control)
+{
+ const auto traj = autoware::test_utils::generateTrajectory<Trajectory>(10, 1.0);
+
+ { // normal case
+ const auto odom = makeOdometry(0.0, 0.0, 0.0);
+ const auto target_longitudinal_vel = 1.0;
+ const size_t closest_traj_point_idx = 0;
+
+ const auto result =
+ calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx);
+
+ EXPECT_DOUBLE_EQ(result.steering_tire_angle, 0.0f);
+ }
+
+ { // lookahead distance exceeds remaining trajectory length
+ const auto odom = makeOdometry(0.0, 0.0, 0.0);
+ const auto target_longitudinal_vel = 2.0;
+ const size_t closest_traj_point_idx = 8;
+
+ const auto result =
+ calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx);
+
+ EXPECT_DOUBLE_EQ(result.steering_tire_angle, 0.0f);
+ }
+}
+} // namespace autoware::control::simple_pure_pursuit