feat(control): stanley control

.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

aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml

@@ -14,5 +14,20 @@
       <remap from="input/trajectory" to="/planning/output/mppi_planned_path"/>
       <remap from="output/control_cmd" to="/control/command/control_cmd"/>
     </node>
+    <!-- stanley control -->
+    <!-- <node pkg="stanley_control" exec="stanley_control" name="stanley_control_node" output="screen">
+      <param name="speed_proportional_gain" value="2.14"/>
+      <param name="external_target_vel" value="8.0"/>
+      <param name="k_gain" value="2.0"/>
+      <param name="k_gain_slow" value="1.0"/>
+      <param name="speed_proportional_gain" value="1.0"/>
+      <remap from="input/kinematics" to="/localization/kinematic_state"/>
+      <remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
+      <remap from="output/control_cmd" to="/control/command/control_cmd"/>
+    </node> -->
+    <!-- speed_proportional_gain: P gain for speed control -->
+    <!-- external_target_vel: Target speed -->
+    <!-- k_gain: Gain to determine additional steering based on speed -->
+    <!-- k_gain_slow: Constant to prevent zero division -->
   </group>
 </launch>

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

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 <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+#include <visualization_msgs/msg/marker.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <rclcpp/rclcpp.hpp>
+#include <cmath>
+
+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<Odometry>::SharedPtr sub_kinematics_;
+            rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
+
+            // tf2 listeners
+            tf2_ros::Buffer tf_buffer;
+            tf2_ros::TransformListener tf_listener;
+
+            // publishers
+            rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_cmd_;
+            rclcpp::Publisher<Float64>::SharedPtr pub_angle_;
+            rclcpp::Publisher<Marker>::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<rclcpp::Parameter> &parameters);
+    };
+} // namespace stanley_control
+
+
+#endif //STANLEY_HPP_

aichallenge/workspace/src/aichallenge_submit/stanley_control/package.xml

@@ -0,0 +1,26 @@
+<?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>stanley_control</name>
+  <version>0.0.1</version>
+  <description>Stanley control for the people</description>
+  <maintainer email="[email protected]">Shotaro Itahara</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>motion_utils</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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 <motion_utils/motion_utils.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
+
+#include <tf2/utils.h>
+
+#include <algorithm>
+
+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<float>("speed_proportional_gain", 2.14)),
+    external_target_vel_(declare_parameter<float>("external_target_vel", 10.0)),
+    k_gain(declare_parameter<float>("k_gain", 2.0)),
+    k_gain_slow(declare_parameter<float>("k_gain_slow", 1.0)){
+        pub_cmd_ = create_publisher<AckermannControlCommand>("output/control_cmd", 1.0);
+        pub_marker_ = create_publisher<Marker>("debug/forward_point", 1);
+
+        pub_angle_ = create_publisher<Float64>("output/angle", 1);
+
+        sub_kinematics_ = create_subscription<Odometry>(
+            "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; });
+        sub_trajectory_ = create_subscription<Trajectory>(
+            "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<rclcpp::Parameter> &parameters){
+        auto result = rcl_interfaces::msg::SetParametersResult();
+        result.successful = true;
+
+        for (const auto &parameter : 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<stanley_control::StanleyControl>());
+    rclcpp::shutdown();
+    return 0;
+}