feat: add autoware_simple_pure_pursuit package

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
+)

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
+
+![Flowchart](https://www.plantuml.com/plantuml/png/LOuxSWCn28PxJa5fNy5Rn4NkiKCaB3D1Q4T2XMyVeZZEH8q6_iV7TJXrdrN1nPMnsUvIkSFQ0roSFlcTd3QG6vvaO8u1ErD-l9tHxsnuUl0u0-jWG1pU3c3BSWCelSq3KvYTzzJCUzFuQoNBOVqk32tTEMDffF_xCDxbc1yguxvQyPdSbhGuY3-aS2RIj6kp8Zwp6EalS7kfmvcxMDd9Yl86aSLr8i0Bz0pziM21hk6TLRy0)
+
+## 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") }}

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

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>

control/autoware_simple_pure_pursuit/package.xml

@@ -0,0 +1,31 @@
+<?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="[email protected]">Yuki Takagi</maintainer>
+  <maintainer email="[email protected]">Kosuke Takeuchi</maintainer>
+  <maintainer email="[email protected]">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_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>

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
+    }
+  }
+}

control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp

@@ -0,0 +1,148 @@
+// 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/universe_utils/geometry/pose_deviation.hpp>
+
+#include <tf2/utils.h>
+
+#include <algorithm>
+
+namespace autoware::control::simple_pure_pursuit
+{
+using autoware::motion_utils::findNearestIndex;
+using autoware::universe_utils::calcLateralDeviation;
+using autoware::universe_utils::calcYawDeviation;
+
+SimplePurePursuitNode::SimplePurePursuitNode(const rclcpp::NodeOptions & node_options)
+: Node("simple_pure_pursuit", node_options),
+  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"))
+{
+  pub_control_command_ =
+    create_publisher<autoware_control_msgs::msg::Control>("~/output/control_command", 1);
+
+  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_.takeData();
+  const auto traj_ptr = traj_sub_.takeData();
+  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)

control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp

@@ -0,0 +1,77 @@
+// 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/universe_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::universe_utils::InterProcessPollingSubscriber<Odometry> odom_sub_{
+    this, "input/odometry"};
+  autoware::universe_utils::InterProcessPollingSubscriber<Trajectory> traj_sub_{
+    this, "input/odometry"};
+
+  // 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_