#7 Find basic parameter value for purepursuit and stanley

SEA-ME-COSS · Apr 13, 2024 · 70198d5 · 70198d5
1 parent d7189df
commit 70198d5
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 25 deletions.
diff --git a/algorithm/src/controller/stanley.cpp b/algorithm/src/controller/stanley.cpp
@@ -2,6 +2,9 @@
 
 Stanley::Stanley(std::vector<std::vector<double>> route, double resolution, double speed, std::vector<double> cpoint,
                 double k, double ks) {
+    // Vehicle Config
+    this->WB = this->car.WB/resolution;
+
     // Vehicle Status
     this->x = cpoint[0];
     this->y = cpoint[1];
@@ -19,9 +22,6 @@ Stanley::Stanley(std::vector<std::vector<double>> route, double resolution, doub
     // this->ki = this->car.ki;
     // this->kd = this->car.kd;
 
-    // Vehicle Config
-    this->WB = this->car.WB/resolution;
-
     // System Config
     this->dt = 0.1;
 

diff --git a/algorithm/src/main.cpp b/algorithm/src/main.cpp
@@ -2,7 +2,7 @@
 #include "map/hybrid_astar_map.hpp"
 #include "hybrid-a-star/a_star.hpp"
 
-#include "controller/pure_pursuit.hpp"
+// #include "controller/pure_pursuit.hpp"
 #include "controller/stanley.hpp"
 
 #include "utils/matplotlibcpp.h"
@@ -60,8 +60,8 @@ int main() {
     // std::vector<std::vector<double>> control_path = purepursuit.getTrajectory();
     // std::vector<int> target_nodes = purepursuit.getTargetnode();
 
-    double k = 0.2;
-    double ks = 1.0;
+    double k = 10.0;
+    double ks = 20.0;
 
     auto stanley = Stanley(route, resolution, speed, cpoint, k, ks);
     stanley.stanley_control();
@@ -115,21 +115,21 @@ int main() {
         // Show path
         plt::plot(path_x, path_y, "r-");
 
-        // Show Control
-        if(i < control_path.size()) {
-            std::vector<int> path_x = {static_cast<int>(control_path[i][0])};
-            std::vector<int> path_y = {static_cast<int>(control_path[i][1])};
-            plt::plot(path_x, path_y, "bo");
-        }
-
-        if(i < target_nodes.size()) {
-            int target_node_index = target_nodes[i];
-            if(target_node_index < route.size()) {
-                std::vector<int> target_x = {static_cast<int>(route[target_node_index][0])};
-                std::vector<int> target_y = {static_cast<int>(route[target_node_index][1])};
-                plt::plot(target_x, target_y, "go");
-            }
-        }
+        // // Show Control
+        // if(i < control_path.size()) {
+        //     std::vector<int> path_x = {static_cast<int>(control_path[i][0])};
+        //     std::vector<int> path_y = {static_cast<int>(control_path[i][1])};
+        //     plt::plot(path_x, path_y, "bo");
+        // }
+
+        // if(i < target_nodes.size()) {
+        //     int target_node_index = target_nodes[i];
+        //     if(target_node_index < route.size()) {
+        //         std::vector<int> target_x = {static_cast<int>(route[target_node_index][0])};
+        //         std::vector<int> target_y = {static_cast<int>(route[target_node_index][1])};
+        //         plt::plot(target_x, target_y, "go");
+        //     }
+        // }
 
         plt::pause(0.01);
     }

diff --git a/control_ws/src/pure_pursuit/src/control.cpp b/control_ws/src/pure_pursuit/src/control.cpp
@@ -1,7 +1,7 @@
 #include "control.hpp"
 
 Control::Control() : rclcpp::Node("vehicle_control") {
-    this->controller = PurePursuit(2.7, 0.5, 4.0);
+    this->controller = PurePursuit(2.7, 1.0, 10.0);
 
     // Subscribe
     path_subscription_ = this->create_subscription<nav_msgs::msg::Path>(
@@ -74,7 +74,7 @@ void Control::publisher_timer_callback() {
 
 void Control::publish_drive(float speed, float steer) {
     auto drive_msg = std::make_unique<geometry_msgs::msg::Twist>();
-    drive_msg->linear.x = speed * 0.8;
+    drive_msg->linear.x = speed * 0.737;
     drive_msg->angular.z = steer;
     std::cout << "Publish Throttle : " << speed << " Publish Steering : " << steer << std::endl;
     this->drive_publisher_->publish(std::move(drive_msg));

diff --git a/control_ws/src/stanley/src/control.cpp b/control_ws/src/stanley/src/control.cpp
@@ -2,7 +2,7 @@
 
 Control::Control() : rclcpp::Node("vehicle_control") {
     double k = 0.5;
-    double ks = 1.0;
+    double ks = 10.0;
 
     this->controller = Stanley(k, ks);
 
@@ -76,7 +76,7 @@ void Control::publisher_timer_callback() {
 
 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->linear.x = speed*0.737;
     drive_msg->angular.z = steer;
     // std::cout << "Publish Throttle : " << speed << " Publish Steering : " << steer << std::endl;
     this->drive_publisher_->publish(std::move(drive_msg));