diff --git a/control/autoware_simple_pure_pursuit/CMakeLists.txt b/control/autoware_simple_pure_pursuit/CMakeLists.txt new file mode 100644 index 00000000..72f8f602 --- /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 00000000..ef9343dc --- /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 + +![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") }} \ No newline at end of file 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 00000000..f78c8639 --- /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 00000000..7bd6e7e0 --- /dev/null +++ b/control/autoware_simple_pure_pursuit/launch/simple_pure_pursuit.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/control/autoware_simple_pure_pursuit/package.xml b/control/autoware_simple_pure_pursuit/package.xml new file mode 100644 index 00000000..59457836 --- /dev/null +++ b/control/autoware_simple_pure_pursuit/package.xml @@ -0,0 +1,31 @@ + + + + autoware_simple_pure_pursuit + 0.0.0 + The autoware_simple_pure_pursuit package + Yuki Takagi + Kosuke Takeuchi + Takayuki Murooka + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_control_msgs + autoware_motion_utils + autoware_planning_msgs + autoware_test_utils + autoware_vehicle_info_utils + rclcpp + rclcpp_components + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + 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 00000000..76b353fa --- /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 00000000..8645cdfc --- /dev/null +++ b/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 +#include + +#include + +#include + +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("lookahead_gain")), + lookahead_min_distance_(declare_parameter("lookahead_min_distance")), + speed_proportional_gain_(declare_parameter("speed_proportional_gain")), + use_external_target_vel_(declare_parameter("use_external_target_vel")), + external_target_vel_(declare_parameter("external_target_vel")) +{ + pub_control_command_ = + create_publisher("~/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(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 00000000..86390e26 --- /dev/null +++ b/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 +#include +#include + +#include +#include +#include +#include + +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 odom_sub_{ + this, "input/odometry"}; + autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ + this, "input/odometry"}; + + // publishers + rclcpp::Publisher::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 00000000..9bdbe283 --- /dev/null +++ b/control/autoware_simple_pure_pursuit/test/test_simple_pure_pursuit.cpp @@ -0,0 +1,153 @@ +// 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 +#include + +#include + +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(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 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(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(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(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(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 \ No newline at end of file