#4 Merge purepursuit in ROS2

control/include/control/control.h

@@ -2,6 +2,8 @@
 
 #include "msg_structs.h"
 
+#include "controller/pure_pursuit.hpp"
+
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "std_msgs/msg/int8.hpp"
@@ -22,6 +24,8 @@ class Control : public rclcpp::Node {
     Control();
 
 private:
+    PurePursuit controller;
+
     std::vector<Pose> refPoses;
     Pose currPose;
     int currVel;

control/include/control/controller/pure_pursuit.hpp

@@ -0,0 +1,60 @@
+#pragma once
+
+#include <array>
+#include <cmath>
+#include <iostream>
+#include <unistd.h>
+
+class PurePursuit {
+
+public:
+    PurePursuit(std::vector<std::vector<int>> route, double speed, std::vector<int> cpoint, double WB, double Kdd, double Ldc);
+
+    void purepursuit_control();
+
+    double getThrottle();
+    double getDelta();
+
+private:
+    // Vehicle State
+    double x;
+    double y;
+    double yaw;
+    double v;
+    double acceleration;
+
+    double rear_x;
+    double rear_y;
+    double WB;
+
+    // PID 
+    double pid_integral;
+    double previous_error;
+    double kp;
+    // double ki;
+    // double kd;
+
+    double dt;
+
+    // Target Node Searching
+    double Ld;
+    double Kdd;
+    double Ldc;
+
+    // purepursuit
+    double delta;
+
+    // Path
+    std::vector<std::vector<double>> route;
+    int route_size;
+
+    // Target Status
+    double target_speed;
+    int target_node;
+
+    double pid_speed_control();
+    double purepursuit_steer_calc();
+    int update_target_node();
+    void update_state(double x, double y, double yaw, double v);
+    double calc_distance(int point_x, int point_y);
+};

control/include/control/msg_structs.h

@@ -6,4 +6,5 @@ struct Pose {
     float x;
     float y;
     float heading;
+    float v;
 };

control/src/control.cpp

@@ -6,8 +6,8 @@ Control::Control() : rclcpp::Node("vehicle_control") {
         "/planner/path", 10, std::bind(&Control::path_callback, this,  std::placeholders::_1));
     velocity_subscription_ = this->create_subscription<std_msgs::msg::Int8>(
         "/planner/velocity", 10, std::bind(&Control::velocity_callback, this,  std::placeholders::_1));
-    // pose_subscription_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
-    //     "/localization/pose", 10, std::bind(&Control::pose_callback, this,  std::placeholders::_1));
+    pose_subscription_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+        "/localization/pose", 10, std::bind(&Control::pose_callback, this,  std::placeholders::_1));
 
     // Publish
     drive_publisher_ = this->create_publisher<ackermann_msgs::msg::AckermannDriveStamped>(
@@ -25,10 +25,9 @@ void Control::path_callback(const nav_msgs::msg::Path::SharedPtr path_msg) {
     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.heading = this->quat_to_yaw(path_msg->poses[i].pose.orientation);
+        // refPose.heading = this->quat_to_yaw(path_msg->poses[i].pose.orientation);
         this->refPoses.push_back(refPose);
     }
-    // this->controller.resetIndex(0);
 }
 
 void Control::velocity_callback(const std_msgs::msg::Int8::SharedPtr velocity_msg) {
@@ -39,23 +38,27 @@ void Control::velocity_callback(const std_msgs::msg::Int8::SharedPtr velocity_ms
     this->velocityValid = true;
 }
 
-// void Control::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr pose_msg) {
-//     this->poseValid = false;
-//     this->currPose.x = pose_msg->pose.position.x;
-//     this->currPose.y = pose_msg->pose.position.y;
-//     this->currPose.heading = this->quat_to_yaw(pose_msg->pose.orientation);
-//     this->poseValid = true;
-// }
+void Control::pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr pose_msg) {
+    this->poseValid = false;
+    this->currPose.x = pose_msg->pose.position.x;
+    this->currPose.y = pose_msg->pose.position.y;
+    this->currPose.heading = this->quat_to_yaw(pose_msg->pose.orientation);
+    this->poseValid = true;
+}
 
 void Control::publisher_timer_callback() {
-    // if (this->refPoses.empty() || !this->poseValid || !this->velocityValid) {return;}
+    if (this->refPoses.empty() || !this->poseValid || !this->velocityValid) {return;}
+
+    path_callback();
+    velocity_callback();
+    pose_callback();
+
+    this->controller.purepursuit_control(this->currPose.x, this->currPose.y, this->currPose.heading, this->v);
 
-    // // this->controller.calculateControl(refPoses, currPose, currVel, currDirection);
-    // this->speedCommand = this->controller.getSpeedCommand();
-    // this->steerCommand = - this->controller.getSteerCommand() * 180 / M_PI;
-    // // this->steerCommand = 0.5*this->controller.getSteerCommand() + 0.5*this->steerCommand;
+    this->speedCommand = this->controller.getThrottle();
+    this->steerCommand = - this->controller.getDelta() * 180 / M_PI;
 
-    // this->publish_drive(this->speedCommand, this->steerCommand);
+    this->publish_drive(this->speedCommand, this->steerCommand);
 }
 
 void Control::publish_drive(float speed, float steer) {

control/src/controller/pure_pursuit.cpp

@@ -0,0 +1,118 @@
+PurePursuit::PurePursuit(std::vector<std::vector<int>> route, double speed, std::vector<double> cpoint, double WB, double Kdd, double Ldc) {
+    // Vehicle Status Init
+    this->x = cpoint[0];
+    this->y = cpoint[1];
+    this->yaw = cpoint[2];
+    this->v = 0.0;
+    this->acceleration = 0.0;
+
+    this->WB = WB;
+    this->rear_x = this->x - ((WB / 2) * cos(this->yaw));
+    this->rear_y = this->y - ((WB / 2) * sin(this->yaw));
+
+    // PID Init
+    this->pid_integral = 0.0;
+    this->previous_error = 0.0;
+    this->kp = 1;
+    // this->ki = 0.1;
+    // this->kd = 0.01;
+
+    this->dt = 0.1;
+
+    // Target Node Searching Init
+    this->Ldc = Ldc;
+    this->Kdd = Kdd;
+    this->Ld = this->Kdd * this->v + this->Ldc;
+
+    // PurePursuit Init
+    this->delta = 0.0;
+
+    // Path Init
+    this->route = route;
+    this->route_size = route.size();
+
+    // Target Status Init
+    this->target_speed = speed;
+    this->target_node = 0;
+}
+
+void PurePursuit::purepursuit_control(double x, double y, double yaw, double v) {
+    this->acceleration = this->pid_speed_control();
+    this->delta = this->purepursuit_steer_calc();
+    this->target_node = this->update_target_node();
+    this->update_state(x, y, yaw, v);
+}
+
+double PurePursuit::pid_speed_control() {
+    // double error = target_speed - v;
+    // pid_integral += error * dt;
+    // double derivative = (error - previous_error)/dt;
+    // double acceleration = kp * error + ki * pid_integral + kd * derivative;
+    // previous_error = error;
+
+    double acceleration = kp * (target_speed - v);
+
+    return acceleration;
+}
+
+double PurePursuit::purepursuit_steer_calc() {
+    double tx = route[this->target_node][0];
+    double ty = route[this->target_node][1];
+
+    double alpha = atan2(ty - this->rear_y, tx - this->rear_x) - this->yaw;
+    double delta = atan2(2.0 * this->WB * sin(alpha)/this->Ld, 1.0);
+
+    return delta;
+}
+
+int PurePursuit::update_target_node() {
+    int cnode = this->target_node;
+    if(cnode == this->route_size-1) { return cnode; }
+
+    double distance_cnode = this->calc_distance(this->route[cnode][0], this->route[cnode][1]);
+    double distance_nnode = 0.0;
+
+    while (cnode < this->route.size()-1) {
+        distance_nnode = this->calc_distance(this->route[cnode + 1][0], this->route[cnode + 1][1]);
+        if (distance_cnode < distance_nnode) {
+            break;
+        }
+        cnode += 1;
+        distance_cnode = distance_nnode;
+    }
+
+    this->Ld = this->Kdd * this->v + this->Ldc;
+
+    if(cnode < this->route_size-1) {
+        while(this->Ld > this->calc_distance(this->route[cnode][0], this->route[cnode][1])) {
+            cnode += 1;
+        }
+    }
+
+    return cnode;
+}
+
+void PurePursuit::update_state(double x, double y, double yaw, double v) {
+    this->x = x;
+    this->y = y;
+    this->yaw = yaw;
+    this->v = v;
+
+    rear_x = x - ((WB / 2) * cos(yaw));
+    rear_y = y - ((WB / 2) * sin(yaw));
+}
+
+double PurePursuit::calc_distance(int point_x, int point_y) {
+    double dx = this->rear_x - point_x;
+    double dy = this->rear_y - point_y;
+
+    return sqrt(dx * dx + dy * dy);
+}
+
+double PurePursuit::getThrottle() {
+    return v;
+}
+
+double PurePursuit::getDelta() {
+    return delta;
+}