#8 update stanley match with ROS2

control_ws/src/pure_pursuit/CMakeLists.txt

@@ -8,7 +8,7 @@ endif()
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 20)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

control_ws/src/stanley/CMakeLists.txt

@@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD)
   set(CMAKE_C_STANDARD 99)
 endif()
 
-# Default to C++14
+# Default to C++20
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 20)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -17,9 +17,12 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(example_interfaces REQUIRED)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -33,3 +36,27 @@ if(BUILD_TESTING)
 endif()
 
 ament_package()
+
+include_directories(
+  include/${PROJECT_NAME}
+)
+
+add_executable(${PROJECT_NAME}
+  src/main.cpp
+  src/control.cpp
+  src/controller/stanley.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME} 
+  rclcpp
+  nav_msgs 
+  tf2 
+  tf2_geometry_msgs 
+  geometry_msgs 
+  example_interfaces
+)
+
+install(TARGETS
+  ${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)

control_ws/src/stanley/include/stanley/control.hpp

@@ -0,0 +1,56 @@
+#pragma once
+
+#include "utils/msg_structs.h"
+#include "utils/car_struct.h"
+
+#include "controller/stanley.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/path.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "example_interfaces/msg/float64.hpp"
+
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+#include "tf2/convert.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+
+#include <string>
+#include <cmath>
+
+class Control : public rclcpp::Node {
+public:
+    Control();
+
+private:
+    Stanley controller;
+
+    std::vector<Path> refPoses;
+    Pose currPose;
+
+    float speedCommand;
+    float steerCommand;
+
+    bool pathValid;
+    bool poseValid;
+    bool velocityValid;
+
+    float target_velocity;
+
+    // Subscribe
+    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_subscription_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr pose_subscription_;
+    rclcpp::Subscription<example_interfaces::msg::Float64>::SharedPtr velocity_subscription_;
+
+    // Publish
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr drive_publisher_;
+
+    rclcpp::TimerBase::SharedPtr publisher_timer_;
+
+    void path_callback(const nav_msgs::msg::Path::SharedPtr path_msg);
+    void pose_callback(const nav_msgs::msg::Odometry::SharedPtr pose_msg);
+    void velocity_callback(const example_interfaces::msg::Float64::SharedPtr velocity_msg);
+    void publisher_timer_callback();
+    void publish_drive(float speed, float steer);
+    float quat_to_yaw(const geometry_msgs::msg::Quaternion quat_msg);
+};

control_ws/src/stanley/include/stanley/controller/stanley.hpp

@@ -0,0 +1,78 @@
+#pragma once
+
+#include "utils/car_struct.h"
+#include "utils/msg_structs.h"
+
+#include <array>
+#include <vector>
+#include <cmath>
+#include <iostream>
+#include <limits>
+#include <vector>
+#include <unistd.h>
+
+class Stanley {
+
+public:
+    Stanley(double k, double ks);
+
+    Stanley();
+    ~Stanley();
+
+    void stanley_control(std::vector<Path> refPoses, double target_v, double x, double y, double yaw, double v);
+
+    double getThrottle();
+    double getDelta();
+
+private:
+    // Struct
+    Car car;
+
+    // Vehicle State    
+    double x;
+    double y;
+    double yaw;
+    double v;
+    double acceleration;
+
+    double front_x;
+    double front_y;
+
+    // PID 
+    // double pid_integral;
+    // double previous_error;
+    double kp;
+    // double ki;
+    // double kd;
+
+    // Vehicle Config
+    double WB;
+
+    // System Config
+    double dt;
+
+    // Stanley Parameters
+    double k;
+    double ks;
+
+    // Path
+    std::vector<Path> refPoses;
+
+    // Target Status
+    double target_speed;
+    int target_node;
+    int previous_node;
+    double delta;
+
+    // etc
+    double node_mindistance;
+
+    void update_state(double x, double y, double yaw, double v);
+    double pid_speed_control();
+    void update_target_node();
+    double stanley_steer_calc();
+    double pi_2_pi(double angle);
+    bool isNearby(std::vector<double> cpoint, std::vector<double> target_point); 
+
+    void stanley_throttle_calc();
+};

control_ws/src/stanley/include/stanley/utils/car_struct.h

@@ -0,0 +1,20 @@
+#pragma once
+
+struct Car {
+    // PID Config
+    double kp = 1.0;
+    double ki = 1.0;
+    double kd = 1.0;
+
+    // Vehicle Config
+    double RF = 3.3;    // [cm] distance from rear to vehicle front end of vehicle
+    double RB = 0.8;    // [cm] distance from rear to vehicle back end of vehicle
+    double W = 2.4;     // [cm] width of vehicle
+    double WD = 0.7*W;  // [cm] distance between left-right wheels
+    double WB = 2.5;    // [cm] Wheel base
+    double TR = 0.44;   // [cm] Tyre radius
+    double TW = 0.7;    // [cm] Tyre width
+
+    double MAX_STEER = 0.65;    // [rad] Maximum steering angle
+    double MAX_SPEED = 10.0;    // [cm/s] Maximum speed
+};

control_ws/src/stanley/include/stanley/utils/msg_structs.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include <string>
+
+struct Path {
+    float x;
+    float y;
+    float yaw;
+};
+
+struct Pose {
+    float x;
+    float y;
+    float yaw;
+    float v;
+};

control_ws/src/stanley/package.xml

@@ -9,6 +9,13 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>example_interfaces</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

control_ws/src/stanley/src/control.cpp

@@ -0,0 +1,94 @@
+#include "control.hpp"
+
+Control::Control() : rclcpp::Node("vehicle_control") {
+    double k = 0.5;
+    double ks = 1.0;
+
+    this->controller = Stanley(k, ks);
+
+    // Subscribe
+    path_subscription_ = this->create_subscription<nav_msgs::msg::Path>(
+        "/pathplanner/path", 10, std::bind(&Control::path_callback, this,  std::placeholders::_1));
+    velocity_subscription_ = this->create_subscription<example_interfaces::msg::Float64>(
+        "/pathplanner/throttle", 10, std::bind(&Control::velocity_callback, this, std::placeholders::_1));
+    pose_subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
+        "/piracer/odom", 10, std::bind(&Control::pose_callback, this,  std::placeholders::_1));
+
+    // Publish
+    drive_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
+        "/piracer/cmd_vel", 10);
+
+    publisher_timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(100),
+        std::bind(&Control::publisher_timer_callback, this)
+    );
+}
+
+void Control::path_callback(const nav_msgs::msg::Path::SharedPtr path_msg) {
+    this->pathValid = false;
+    this->refPoses.clear();
+    Path refPose;
+    for (size_t i = 0; i < path_msg->poses.size(); ++i) {
+        refPose.x = path_msg->poses[i].pose.position.x;
+        refPose.y = path_msg->poses[i].pose.position.y;
+        refPose.yaw = this->quat_to_yaw(path_msg->poses[i].pose.orientation);
+        this->refPoses.push_back(refPose);
+    }
+    this->pathValid = true;
+}
+
+void Control::velocity_callback(const example_interfaces::msg::Float64::SharedPtr velocity_msg) {
+    this->velocityValid = false;
+    this->target_velocity = velocity_msg->data;
+    this->velocityValid = true;
+}
+
+void Control::pose_callback(const nav_msgs::msg::Odometry::SharedPtr pose_msg) {
+    this->poseValid = false;
+    this->currPose.x = pose_msg->pose.pose.position.x * 100 + 22.656;  // [cm]
+    this->currPose.y = pose_msg->pose.pose.position.y * 100 + 47.281;  // [cm]
+
+    tf2::Quaternion q(
+      pose_msg->pose.pose.orientation.x,
+      pose_msg->pose.pose.orientation.y,
+      pose_msg->pose.pose.orientation.z,
+      pose_msg->pose.pose.orientation.w);
+    tf2::Matrix3x3 m(q);
+    double roll, pitch, yaw;
+    m.getRPY(roll, pitch, yaw);  // [rad]
+    this->currPose.yaw = yaw;
+
+    this->currPose.v = sqrt(pow(pose_msg->twist.twist.linear.x, 2)
+                        + pow(pose_msg->twist.twist.linear.y, 2)) * 5;  // [cm/s]
+    this->poseValid = true;
+}
+
+void Control::publisher_timer_callback() {
+    if (this->refPoses.empty() || !this->poseValid || !this->pathValid) { std::cout << "Message Receive Error" << std::endl; return;}
+
+    this->controller.stanley_control(this->refPoses, this->target_velocity, this->currPose.x, this->currPose.y, this->currPose.yaw, this->currPose.v);
+
+    this->speedCommand = this->controller.getThrottle();
+    this->steerCommand = this->controller.getDelta();
+
+    this->publish_drive(this->speedCommand, this->steerCommand);
+}
+
+void Control::publish_drive(float speed, float steer) {
+    auto drive_msg = std::make_unique<geometry_msgs::msg::Twist>();
+    drive_msg->linear.x = speed;
+    drive_msg->angular.z = steer;
+    // std::cout << "Publish Throttle : " << speed << " Publish Steering : " << steer << std::endl;
+    this->drive_publisher_->publish(std::move(drive_msg));
+}
+
+float Control::quat_to_yaw(const geometry_msgs::msg::Quaternion quat) {
+    tf2::Quaternion tf2_quat;
+    tf2::fromMsg(quat, tf2_quat);
+    double roll;
+    double pitch;
+    double yaw;
+    tf2::Matrix3x3 matrix(tf2_quat);
+    matrix.getRPY(roll, pitch, yaw);
+    return yaw;
+}