From 43182d17f4826aace39113320e01fa2bbf86aa7f Mon Sep 17 00:00:00 2001 From: sitahara <33857284+sitahara@users.noreply.github.com> Date: Wed, 4 Sep 2024 23:13:45 +0900 Subject: [PATCH] feat(control): stanley control (#25) * feat(control): stanley control * chore: fix spelling * chore: add myself in CODEOWNERS file * Update aichallenge/workspace/src/aichallenge_submit/stanley_control/src/stanley_control.cpp Consistency of indent Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> * fix: fix parameter names on launch and code * feat: add parameter descriptions --------- Co-authored-by: Shotaro Itahara Co-authored-by: Autumn60 <37181352+Autumn60@users.noreply.github.com> --- .github/CODEOWNERS | 1 + .../launch/components/control.launch.xml | 15 ++ .../stanley_control/CMakeLists.txt | 20 ++ .../stanley_control/stanley_control.hpp | 90 ++++++++ .../stanley_control/package.xml | 26 +++ .../stanley_control/src/stanley_control.cpp | 196 ++++++++++++++++++ 6 files changed, 348 insertions(+) create mode 100644 aichallenge/workspace/src/aichallenge_submit/stanley_control/CMakeLists.txt create mode 100644 aichallenge/workspace/src/aichallenge_submit/stanley_control/include/stanley_control/stanley_control.hpp create mode 100644 aichallenge/workspace/src/aichallenge_submit/stanley_control/package.xml create mode 100644 aichallenge/workspace/src/aichallenge_submit/stanley_control/src/stanley_control.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 13913d57..1cf6e6e9 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -13,3 +13,4 @@ aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/ aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/** @sitahara aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/** @booars/aic2024-developers aichallenge/workspace/src/aichallenge_submit/mppi_controller/** @tamago117 +aichallenge/workspace/src/aichallenge_submit/stanley_control/** @sitahara diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml index d39ec7b6..6458d746 100644 --- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml +++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml @@ -14,5 +14,20 @@ + + + + + + \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/stanley_control/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/stanley_control/CMakeLists.txt new file mode 100644 index 00000000..6d33c02c --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/stanley_control/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.8) +project(stanley_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(autoware_cmake REQUIRED) +autoware_package() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +ament_auto_add_executable(stanley_control + src/stanley_control.cpp +) + +ament_auto_package() diff --git a/aichallenge/workspace/src/aichallenge_submit/stanley_control/include/stanley_control/stanley_control.hpp b/aichallenge/workspace/src/aichallenge_submit/stanley_control/include/stanley_control/stanley_control.hpp new file mode 100644 index 00000000..9ff5f6cd --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/stanley_control/include/stanley_control/stanley_control.hpp @@ -0,0 +1,90 @@ +// Copyright 2024 Booars +// +// 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 STANLEY_HPP_ +#define STANLEY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace stanley_control{ + using autoware_auto_control_msgs::msg::AckermannControlCommand; + using autoware_auto_planning_msgs::msg::Trajectory; + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using geometry_msgs::msg::Pose; + using geometry_msgs::msg::Twist; + using geometry_msgs::msg::Vector3Stamped; + using visualization_msgs::msg::Marker; + using std_msgs::msg::Float64; + using nav_msgs::msg::Odometry; + + class StanleyControl : public rclcpp::Node { + public: + explicit StanleyControl(); + + // subscribers + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + + // tf2 listeners + tf2_ros::Buffer tf_buffer; + tf2_ros::TransformListener tf_listener; + + // publishers + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_angle_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // timer for control + rclcpp::TimerBase::SharedPtr timer_; + + // updated by subscribers + Trajectory::SharedPtr trajectory_; + Odometry::SharedPtr odometry_; + + // stanley parameters + // speed control + double speed_proportional_gain_; + double external_target_vel_; + // stanley parameters + double k_gain; + double k_gain_slow; + + private: + void onTimer(); + bool subscribeMessageAvailable(); + + + // control variables + double error_distance; + double track_yaw; + OnSetParametersCallbackHandle::SharedPtr reset_param_handler_; + rcl_interfaces::msg::SetParametersResult parameter_callback(const std::vector ¶meters); + }; +} // namespace stanley_control + + +#endif //STANLEY_HPP_ \ No newline at end of file diff --git a/aichallenge/workspace/src/aichallenge_submit/stanley_control/package.xml b/aichallenge/workspace/src/aichallenge_submit/stanley_control/package.xml new file mode 100644 index 00000000..9e64164d --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/stanley_control/package.xml @@ -0,0 +1,26 @@ + + + + stanley_control + 0.0.1 + Stanley control for the people + Shotaro Itahara + Apache-2.0 + + ament_cmake + + rclcpp + autoware_auto_control_msgs + autoware_auto_planning_msgs + motion_utils + geometry_msgs + std_msgs + nav_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/aichallenge/workspace/src/aichallenge_submit/stanley_control/src/stanley_control.cpp b/aichallenge/workspace/src/aichallenge_submit/stanley_control/src/stanley_control.cpp new file mode 100644 index 00000000..7bf7d936 --- /dev/null +++ b/aichallenge/workspace/src/aichallenge_submit/stanley_control/src/stanley_control.cpp @@ -0,0 +1,196 @@ +// Copyright 2024 Booars +// +// 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 "stanley_control/stanley_control.hpp" + +#include +#include + +#include + +#include + +namespace stanley_control{ + + using motion_utils::findNearestIndex; + + StanleyControl::StanleyControl(): Node("stanley_control"), tf_buffer(this->get_clock()), tf_listener(tf_buffer), + speed_proportional_gain_(declare_parameter("speed_proportional_gain", 2.14)), + external_target_vel_(declare_parameter("external_target_vel", 10.0)), + k_gain(declare_parameter("k_gain", 2.0)), + k_gain_slow(declare_parameter("k_gain_slow", 1.0)){ + pub_cmd_ = create_publisher("output/control_cmd", 1.0); + pub_marker_ = create_publisher("debug/forward_point", 1); + + pub_angle_ = create_publisher("output/angle", 1); + + sub_kinematics_ = create_subscription( + "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); + sub_trajectory_ = create_subscription( + "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; }); + + + external_target_vel_=10.0; + speed_proportional_gain_=1.0; + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer(this, get_clock(), 10ms, std::bind(&StanleyControl::onTimer, this)); + + // dynamic reconfigure + auto parameter_change_cb = std::bind(&StanleyControl::parameter_callback, this, std::placeholders::_1); + reset_param_handler_ = StanleyControl::add_on_set_parameters_callback(parameter_change_cb); + } + + AckermannControlCommand zeroAckermannControlCommand(rclcpp::Time stamp){ + AckermannControlCommand cmd; + cmd.stamp = stamp; + cmd.longitudinal.stamp = stamp; + cmd.longitudinal.speed = 0.0; + cmd.longitudinal.acceleration = 0.0; + cmd.lateral.stamp = stamp; + cmd.lateral.steering_tire_angle = 0.0; + return cmd; + } + + void StanleyControl::onTimer(){ + // check data + if (!subscribeMessageAvailable()) { + return; + } + + // create zero command + AckermannControlCommand cmd = zeroAckermannControlCommand(get_clock()->now()); + + // Check if goal is "reached" + size_t closet_traj_point_idx = findNearestIndex(trajectory_->points, odometry_->pose.pose.position); + if ((closet_traj_point_idx == trajectory_->points.size() - 1) || (trajectory_->points.size() <= 5)) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "reached to the goal"); + } + // Otherwise (main control) + else { + // get closest trajectory point from current position + TrajectoryPoint closet_traj_point = trajectory_->points.at(closet_traj_point_idx); + + // longitudinal control + double target_longitudinal_vel = external_target_vel_; + double current_longitudinal_vel = odometry_->twist.twist.linear.x; + + cmd.longitudinal.speed = target_longitudinal_vel; + cmd.longitudinal.acceleration = + speed_proportional_gain_ * (target_longitudinal_vel - current_longitudinal_vel); + + // calc lateral control + //// calculate track yaw relative to the vehicle + + //// calculate track yaw angle and publish it + if (trajectory_->points.size() - (closet_traj_point_idx+1) <= 3 ){ + cmd.lateral.steering_tire_angle=0.0; + } + else{ + // obtain closest point and next point on the trajectory + size_t next_closest_traj_point_idx = closet_traj_point_idx+1; + TrajectoryPoint next_closet_traj_point = trajectory_->points.at(next_closest_traj_point_idx); + + // calculate track "yaw" angle + //// Trajectory vector + double x_track = next_closet_traj_point.pose.position.x - closet_traj_point.pose.position.x; + double y_track = next_closet_traj_point.pose.position.y - closet_traj_point.pose.position.y; + + double norm_track = sqrt(pow(x_track,2) + pow(y_track,2)); + + //// vehicle heading vector in "map" frame + //// obtain this by transforming constant vector in "base_link" frame to "map" frame + Vector3Stamped vector_fixed, vector_vehicle; + double norm_vehicle=1.0; + { + vector_fixed.header.frame_id = "base_link"; + vector_fixed.vector.x = 1.0; + vector_fixed.vector.y = 0.0; + vector_fixed.vector.z = 0.0; + } + try{ + auto transform = tf_buffer.lookupTransform("map","base_link", tf2::TimePointZero); + tf2::doTransform(vector_fixed, vector_vehicle, transform); + norm_vehicle = sqrt(pow(vector_vehicle.vector.x, 2) + pow(vector_vehicle.vector.y, 2)); + } + catch (tf2::TransformException &ex){ + RCLCPP_ERROR(get_logger(), "Could not obtain transform from map to base_link: %s", ex.what()); + } + + // calculate the cosine of two vectors, from the inner product + // use outer product to calculate signed angle (if v1xv2 >=0, then v2 is "to the left" of v1 ) + double ip_vector = x_track*vector_vehicle.vector.x + y_track*vector_vehicle.vector.y; + double cos_vector = ip_vector / (norm_track * norm_vehicle); + double angle = acos(cos_vector); + + double xp_vector = x_track*vector_vehicle.vector.y - y_track*vector_vehicle.vector.x; + if(xp_vector>=0) angle*=-1; + + // calculate positional error from nearest trajectory point + + error_distance = sqrt(pow(closet_traj_point.pose.position.x-odometry_->pose.pose.position.x,2)+pow(closet_traj_point.pose.position.y-odometry_->pose.pose.position.y,2)); + if ( + (closet_traj_point.pose.position.x-odometry_->pose.pose.position.x)*vector_vehicle.vector.y + - (closet_traj_point.pose.position.y-odometry_->pose.pose.position.y)*vector_vehicle.vector.x >= 0){ + error_distance*=-1; + } + track_yaw = atan2(k_gain*error_distance, odometry_->twist.twist.linear.x+k_gain_slow); + Float64 pub_angle = Float64(); + pub_angle.data = angle; + pub_angle_->publish(pub_angle); + cmd.lateral.steering_tire_angle=angle+track_yaw; + } + } + pub_cmd_->publish(cmd); + } + + bool StanleyControl::subscribeMessageAvailable(){ + if (!odometry_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "odometry is not available"); + return false; + } + if (!trajectory_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000 /*ms*/, "trajectory is not available"); + return false; + } + return true; + } + rcl_interfaces::msg::SetParametersResult StanleyControl::parameter_callback(const std::vector ¶meters){ + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + for (const auto ¶meter : parameters) { + if (parameter.get_name() == "k_gain") { + k_gain = parameter.as_double(); + RCLCPP_INFO(StanleyControl::get_logger(), "k_gain changed to %f", k_gain); + } else if (parameter.get_name() == "k_gain_slow") { + k_gain_slow = parameter.as_double(); + RCLCPP_INFO(StanleyControl::get_logger(), "k_gain_slow changed to %f", k_gain_slow); + } else if (parameter.get_name() == "external_target_vel_") { + external_target_vel_ = parameter.as_double(); + RCLCPP_INFO(StanleyControl::get_logger(), "external_target_vel changed to %f", external_target_vel_); + } + else if (parameter.get_name() == "speed_proportional_gain_") { + speed_proportional_gain_ = parameter.as_double(); + RCLCPP_INFO(StanleyControl::get_logger(), "speed_proportional_gain changed to %f", speed_proportional_gain_); + } + } + return result; + } +} // namespace stanley_control + +int main(int argc, char const * argv[]){ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}