#4 Debugging Fixed

algorithm/main.cpp

@@ -94,14 +94,14 @@ int main() {
             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 = {route[target_node_index][0]};
-        //         std::vector<int> target_y = {route[target_node_index][1]};
-        //         plt::plot(target_x, target_y, "go"); // 목표 노드는 초록색으로
-        //     }
-        // }
+        if(i < target_nodes.size()) {
+            int target_node_index = target_nodes[i];
+            if(target_node_index < route.size()) {
+                std::vector<int> target_x = {route[target_node_index][0]};
+                std::vector<int> target_y = {route[target_node_index][1]};
+                plt::plot(target_x, target_y, "go"); // 목표 노드는 초록색으로
+            }
+        }
 
         plt::pause(0.01);
     }

control/include/control/controller/pure_pursuit.hpp

@@ -59,4 +59,5 @@ class PurePursuit {
     int update_target_node();
     void update_state(double x, double y, double yaw, double v);
     double calc_distance(double point_x, double point_y);
+    void purepursuit_throttle_calc();
 };

control/src/control.cpp

@@ -1,6 +1,6 @@
 #include "control.h"
 
-Control::Control() : rclcpp::Node("vehicle_control"), controller(2.7, 0.5, 4.0) {
+Control::Control() : rclcpp::Node("vehicle_control"), controller(15, 0.5, 10.0) {
     // this->controller = PurePursuit(2.7, 0.5, 4.0);
 
     // Subscribe
@@ -28,7 +28,7 @@ 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.yaw = this->quat_to_yaw(path_msg->poses[i].pose.orientation);
+        // refPose.yaw = this->quat_to_yaw(path_msg->poses[i].pose.orientation);
         this->refPoses.push_back(refPose);
     }
     this->pathValid = true;
@@ -42,8 +42,8 @@ void Control::velocity_callback(const example_interfaces::msg::Float64::SharedPt
 
 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;  // [cm]
-    this->currPose.y = pose_msg->pose.pose.position.y * 100;  // [cm]
+    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,
@@ -56,17 +56,18 @@ void Control::pose_callback(const nav_msgs::msg::Odometry::SharedPtr pose_msg) {
     this->currPose.yaw = yaw;
 
     this->currPose.v = sqrt(pow(pose_msg->twist.twist.linear.x, 2)
-                        + pow(pose_msg->twist.twist.linear.y, 2)) * 100;  // [cm/s]
+                        + 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) {return;}
+    if (this->refPoses.empty() || !this->poseValid || !this->pathValid) { std::cout << "Message Receive Error" << std::endl; return;}
 
     this->controller.purepursuit_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() * 180 / M_PI;
+    // this->steerCommand = - this->controller.getDelta() * 180 / M_PI;
+    this->steerCommand = this->controller.getDelta();
 
     this->publish_drive(this->speedCommand, this->steerCommand);
 }
@@ -75,7 +76,7 @@ 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 << "Throttle : " << speed << " Steering : " << steer << std::endl;
+    std::cout << "Publish Throttle : " << speed << " Publish Steering : " << steer << std::endl;
     this->drive_publisher_->publish(std::move(drive_msg));
 }
 

control/src/controller/pure_pursuit.cpp

@@ -36,10 +36,13 @@ PurePursuit::PurePursuit(double WB, double Kdd, double Ldc) {
 void PurePursuit::purepursuit_control(std::vector<Path> refPoses, double target_v, double x, double y, double yaw, double v) {
     this->target_speed = target_v;
     this->refPoses = refPoses;
+    this->update_state(x, y, yaw, v);
+    std::cout << "Current Speed : " << this->v << " Current accerleration : " << this->acceleration << std::endl;
     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);
+    this->delta = this->purepursuit_steer_calc();
+    this->purepursuit_throttle_calc();
+    std::cout << "After Speed : " << this->v << " After accerleration : " << this->acceleration << std::endl;
 }
 
 double PurePursuit::pid_speed_control() {
@@ -60,11 +63,16 @@ double PurePursuit::purepursuit_steer_calc() {
     // double tyaw = refPoses[this->target_node].yaw;
 
     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);
+    double delta = atan2(2.0 * this->WB * sin(alpha)/this->Ld, 1.0); // rad
 
     return delta;
 }
 
+void PurePursuit::purepursuit_throttle_calc() {
+    this->v += this->acceleration * this->dt;
+    // std::cout << "Accerleration : " << this->acceleration << " V: " << this->v << std::endl;
+} 
+
 int PurePursuit::update_target_node() {
     int cnode = this->target_node;
     if(cnode == this->refPoses.size()-1) { return cnode; }
@@ -98,8 +106,8 @@ void PurePursuit::update_state(double x, double y, double yaw, double v) {
     this->yaw = yaw;
     this->v = v;
 
-    rear_x = x - ((WB / 2) * cos(yaw));
-    rear_y = y - ((WB / 2) * sin(yaw));
+    this->rear_x = x - ((WB / 2) * cos(yaw));
+    this->rear_y = y - ((WB / 2) * sin(yaw));
 }
 
 double PurePursuit::calc_distance(double point_x, double point_y) {