From 0fd67b06aa0ac022b83257f36f147707408a9078 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 24 Dec 2024 18:47:51 +0900 Subject: [PATCH 1/5] feat: introduce simple_pure_pursuit package Signed-off-by: Takayuki Murooka --- .../CMakeLists.txt | 20 +++ .../config/simple_pure_pursuit.param.yaml | 7 + .../autoware_simple_pure_pursuit/package.xml | 22 +++ .../src/simple_pure_pursuit.cpp | 148 ++++++++++++++++++ .../src/simple_pure_pursuit.hpp | 72 +++++++++ 5 files changed, 269 insertions(+) create mode 100644 control/autoware_simple_pure_pursuit/CMakeLists.txt create mode 100644 control/autoware_simple_pure_pursuit/config/simple_pure_pursuit.param.yaml create mode 100644 control/autoware_simple_pure_pursuit/package.xml create mode 100644 control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp create mode 100644 control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp diff --git a/control/autoware_simple_pure_pursuit/CMakeLists.txt b/control/autoware_simple_pure_pursuit/CMakeLists.txt new file mode 100644 index 0000000000..64311b8216 --- /dev/null +++ b/control/autoware_simple_pure_pursuit/CMakeLists.txt @@ -0,0 +1,20 @@ +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 +) + +ament_auto_package( + INSTALL_TO_SHARE + # launch + config +) 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/package.xml b/control/autoware_simple_pure_pursuit/package.xml new file mode 100644 index 0000000000..782998b218 --- /dev/null +++ b/control/autoware_simple_pure_pursuit/package.xml @@ -0,0 +1,22 @@ + + + + autoware_simple_pure_pursuit + 0.0.0 + The autoware_simple_pure_pursuit package + hisaki + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_control_msgs + autoware_motion_utils + autoware_planning_msgs + rclcpp + rclcpp_components + + + ament_cmake + + 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..c877f40a72 --- /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), + wheel_base_(declare_parameter("wheel_base")), + 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_steering_angle(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_steering_angle( + 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 - wheel_base_ / 2.0 * std::cos(odom.pose.pose.orientation.z); + const double rear_y = + odom.pose.pose.position.y - wheel_base_ / 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 * wheel_base_ * 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 0000000000..aa0e99b5b2 --- /dev/null +++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp @@ -0,0 +1,72 @@ +// 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 + +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_; + + // pure pursuit parameters + // TODO(murooka) use vehicle info + const double wheel_base_; + 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_steering_angle( + const Odometry & odom, const Trajectory & traj, const double target_longitudinal_vel, + const size_t closest_traj_point_idx) const; +}; + +} // namespace autoware::control::simple_pure_pursuit + +#endif // SIMPLE_PURE_PURSUIT_HPP_ From da3f7077aa27c82e7b57f194a8f925fce1f9ccf6 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 24 Dec 2024 18:52:46 +0900 Subject: [PATCH 2/5] use vehicle_info_utils Signed-off-by: Takayuki Murooka --- control/autoware_simple_pure_pursuit/package.xml | 6 +++++- .../src/simple_pure_pursuit.cpp | 12 ++++++------ .../src/simple_pure_pursuit.hpp | 6 ++++-- 3 files changed, 15 insertions(+), 9 deletions(-) diff --git a/control/autoware_simple_pure_pursuit/package.xml b/control/autoware_simple_pure_pursuit/package.xml index 782998b218..9243c25dcd 100644 --- a/control/autoware_simple_pure_pursuit/package.xml +++ b/control/autoware_simple_pure_pursuit/package.xml @@ -4,7 +4,10 @@ autoware_simple_pure_pursuit 0.0.0 The autoware_simple_pure_pursuit package - hisaki + Yuki Takagi + Kosuke Takeuchi + Takayuki Murooka + Apache License 2.0 ament_cmake_auto @@ -13,6 +16,7 @@ autoware_control_msgs autoware_motion_utils autoware_planning_msgs + autoware_vehicle_info_utils rclcpp rclcpp_components diff --git a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp index c877f40a72..8d954bb5e7 100644 --- a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -29,7 +29,7 @@ using autoware::universe_utils::calcYawDeviation; SimplePurePursuitNode::SimplePurePursuitNode(const rclcpp::NodeOptions & node_options) : Node("simple_pure_pursuit", node_options), - wheel_base_(declare_parameter("wheel_base")), + 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")), @@ -115,10 +115,10 @@ autoware_control_msgs::msg::Lateral SimplePurePursuitNode::calc_steering_angle( lookahead_gain_ * target_longitudinal_vel + lookahead_min_distance_; // calculate center coordinate of rear wheel - const double rear_x = - odom.pose.pose.position.x - wheel_base_ / 2.0 * std::cos(odom.pose.pose.orientation.z); - const double rear_y = - odom.pose.pose.position.y - wheel_base_ / 2.0 * std::sin(odom.pose.pose.orientation.z); + 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( @@ -138,7 +138,7 @@ autoware_control_msgs::msg::Lateral SimplePurePursuitNode::calc_steering_angle( 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 * wheel_base_ * std::sin(alpha), lookahead_distance); + std::atan2(2.0 * vehicle_info_.wheel_base_m * std::sin(alpha), lookahead_distance); return lateral_control_command; } diff --git a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp index aa0e99b5b2..3181edb113 100644 --- a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp +++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp @@ -16,6 +16,7 @@ #define SIMPLE_PURE_PURSUIT_HPP_ #include +#include #include #include @@ -47,9 +48,10 @@ class SimplePurePursuitNode : public rclcpp::Node // timer rclcpp::TimerBase::SharedPtr timer_; + // vehicle info + const autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + // pure pursuit parameters - // TODO(murooka) use vehicle info - const double wheel_base_; const double lookahead_gain_; const double lookahead_min_distance_; const double speed_proportional_gain_; From 3c7e31fad565f4a53b7761c90f78890dad2cf955 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 24 Dec 2024 19:01:38 +0900 Subject: [PATCH 3/5] add launch Signed-off-by: Takayuki Murooka --- control/autoware_simple_pure_pursuit/CMakeLists.txt | 2 +- .../launch/simple_pure_pursuit.launch.xml | 12 ++++++++++++ .../src/simple_pure_pursuit.cpp | 2 +- .../src/simple_pure_pursuit.hpp | 4 ++-- 4 files changed, 16 insertions(+), 4 deletions(-) create mode 100644 control/autoware_simple_pure_pursuit/launch/simple_pure_pursuit.launch.xml diff --git a/control/autoware_simple_pure_pursuit/CMakeLists.txt b/control/autoware_simple_pure_pursuit/CMakeLists.txt index 64311b8216..c5a4b0e0ed 100644 --- a/control/autoware_simple_pure_pursuit/CMakeLists.txt +++ b/control/autoware_simple_pure_pursuit/CMakeLists.txt @@ -15,6 +15,6 @@ rclcpp_components_register_node(${PROJECT_NAME}_lib ament_auto_package( INSTALL_TO_SHARE - # launch + launch config ) 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 @@ + + + + + + + + + + + + diff --git a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp index 8d954bb5e7..7d3ddff087 100644 --- a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -37,7 +37,7 @@ SimplePurePursuitNode::SimplePurePursuitNode(const rclcpp::NodeOptions & node_op external_target_vel_(declare_parameter("external_target_vel")) { pub_control_command_ = - create_publisher("output/control_command", 1); + create_publisher("~/output/control_command", 1); using namespace std::literals::chrono_literals; timer_ = rclcpp::create_timer( diff --git a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp index 3181edb113..1f6b797a3b 100644 --- a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp +++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp @@ -38,9 +38,9 @@ class SimplePurePursuitNode : public rclcpp::Node private: // subscribers autoware::universe_utils::InterProcessPollingSubscriber odom_sub_{ - this, "~/input/odometry"}; + this, "input/odometry"}; autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ - this, "~/input/odometry"}; + this, "input/odometry"}; // publishers rclcpp::Publisher::SharedPtr pub_control_command_; From 67e3793d1f632298ddd70cf9ccb51eac74227e18 Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Thu, 9 Jan 2025 18:12:53 +0900 Subject: [PATCH 4/5] test(simple_pure_pursuit): add unit tests (#1) * add unit tests Signed-off-by: mitukou1109 * add test for calc_longitudinal_control Signed-off-by: mitukou1109 * rename calc_steering_angle to calc_lateral_control Signed-off-by: mitukou1109 --------- Signed-off-by: mitukou1109 --- .../CMakeLists.txt | 9 ++ .../autoware_simple_pure_pursuit/package.xml | 5 + .../src/simple_pure_pursuit.cpp | 4 +- .../src/simple_pure_pursuit.hpp | 5 +- .../test/test_simple_pure_pursuit.cpp | 153 ++++++++++++++++++ 5 files changed, 173 insertions(+), 3 deletions(-) create mode 100644 control/autoware_simple_pure_pursuit/test/test_simple_pure_pursuit.cpp diff --git a/control/autoware_simple_pure_pursuit/CMakeLists.txt b/control/autoware_simple_pure_pursuit/CMakeLists.txt index c5a4b0e0ed..72f8f60287 100644 --- a/control/autoware_simple_pure_pursuit/CMakeLists.txt +++ b/control/autoware_simple_pure_pursuit/CMakeLists.txt @@ -13,6 +13,15 @@ rclcpp_components_register_node(${PROJECT_NAME}_lib 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 diff --git a/control/autoware_simple_pure_pursuit/package.xml b/control/autoware_simple_pure_pursuit/package.xml index 9243c25dcd..5945783645 100644 --- a/control/autoware_simple_pure_pursuit/package.xml +++ b/control/autoware_simple_pure_pursuit/package.xml @@ -16,10 +16,15 @@ 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/src/simple_pure_pursuit.cpp b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp index 7d3ddff087..8645cdfc0a 100644 --- a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp +++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.cpp @@ -88,7 +88,7 @@ autoware_control_msgs::msg::Control SimplePurePursuitNode::create_control_comman autoware_control_msgs::msg::Control control_command; control_command.longitudinal = calc_longitudinal_control(odom, target_longitudinal_vel); control_command.lateral = - calc_steering_angle(odom, traj, target_longitudinal_vel, closest_traj_point_idx); + calc_lateral_control(odom, traj, target_longitudinal_vel, closest_traj_point_idx); return control_command; } @@ -106,7 +106,7 @@ autoware_control_msgs::msg::Longitudinal SimplePurePursuitNode::calc_longitudina return longitudinal_control_command; } -autoware_control_msgs::msg::Lateral SimplePurePursuitNode::calc_steering_angle( +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 { diff --git a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp index 1f6b797a3b..86390e266c 100644 --- a/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp +++ b/control/autoware_simple_pure_pursuit/src/simple_pure_pursuit.hpp @@ -64,9 +64,12 @@ class SimplePurePursuitNode : public rclcpp::Node 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_steering_angle( + 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 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..9bdbe2835f --- /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 From 07c62c108dcf4f528996c92dec0d22c9b9129e3c Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Thu, 9 Jan 2025 18:13:28 +0900 Subject: [PATCH 5/5] docs(simple_pure_pursuit): add README (#2) * add parameter description Signed-off-by: mitukou1109 * add README Signed-off-by: mitukou1109 --------- Signed-off-by: mitukou1109 --- .../autoware_simple_pure_pursuit/README.md | 24 ++++++++++ .../schema/simple_pure_pursuit.schema.json | 44 +++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 control/autoware_simple_pure_pursuit/README.md create mode 100644 control/autoware_simple_pure_pursuit/schema/simple_pure_pursuit.schema.json diff --git a/control/autoware_simple_pure_pursuit/README.md b/control/autoware_simple_pure_pursuit/README.md new file mode 100644 index 0000000000..ef9343dcf3 --- /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/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 + } + } +}