After some test with Stanley #1

jkk-research · May 24, 2023 · 98970df · 98970df
1 parent 9bbec6d
commit 98970df
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 7 deletions.
diff --git a/src/stanley_control.cpp b/src/stanley_control.cpp
@@ -66,9 +66,11 @@ class StanleyControl : public rclcpp::Node
     this->declare_parameter<std::string>("waypoint_topic", "");
     this->declare_parameter<float>("wheelbase", wheelbase);
     this->declare_parameter<float>("heading_err_rate", heading_err_rate);
+    this->declare_parameter<float>("cross_track_err_rate", cross_track_err_rate);
     this->get_parameter("waypoint_topic", waypoint_topic);
     this->get_parameter("wheelbase", wheelbase);
     this->get_parameter("heading_err_rate", heading_err_rate);
+    this->get_parameter("cross_track_err_rate", cross_track_err_rate);
 
     goal_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/lexus3/cmd_vel", 10);
     reinit_pub_ = this->create_publisher<std_msgs::msg::Bool>("/lexus3/control_reinit", 10);
@@ -91,12 +93,12 @@ class StanleyControl : public rclcpp::Node
     float alpha = atan2(goal_y, goal_x);
     float lookahead_distance = sqrt(pow(goal_x, 2) + pow(goal_y, 2));
     // cross-track error
-    float cross_track_error = atan2(2.0 * wheelbase * sin(alpha) / (lookahead_distance), 1);
-    cross_track_error += cur_cross_track_err * cross_track_err_rate;
+    float purs_steering_angle = atan2(2.0 * wheelbase * sin(alpha) / (lookahead_distance), 1);
+    float cross_track_error = cur_cross_track_err * cross_track_err_rate;
     // heading error
     float heading_error = goal_yaw * heading_err_rate;
-    // RCLCPP_INFO_STREAM(this->get_logger(), "heading: " << std::fixed << std::setprecision(2) << goal_yaw << ", cross-track: " << cross_track_error);
-    return cross_track_error + heading_error;
+    //RCLCPP_INFO_STREAM(this->get_logger(), "heading: " << std::fixed << std::setprecision(2) << goal_yaw << ", cross-track: " << cross_track_error << ", cross-track abs: " << cur_cross_track_abs << " purs_steering_angle" << purs_steering_angle);
+    return purs_steering_angle + cross_track_error + heading_error;
   }
 
   void speedCallback(const std_msgs::msg::Float32 &msg) const
@@ -120,6 +122,7 @@ class StanleyControl : public rclcpp::Node
   void metricsCallback(const std_msgs::msg::Float32MultiArray &metrics_arr)
   {
     cur_cross_track_err = metrics_arr.data[common_wpt::CUR_LAT_DIST_SIGNED];
+    //cur_cross_track_abs = metrics_arr.data[common_wpt::CUR_LAT_DIST_ABS];
   }
 
   void timerLoop()
@@ -144,6 +147,7 @@ class StanleyControl : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr timer_;
   float wheelbase = 2.789; // Lexus3
   float cur_cross_track_err = 0.0;
+  //float cur_cross_track_abs = 0.0;
   OnSetParametersCallbackHandle::SharedPtr callback_handle_;
   float heading_err_rate = 1.0; // TODO: tune
   float cross_track_err_rate = 1.0; // TODO: tune

diff --git a/src/waypoint_to_target.cpp b/src/waypoint_to_target.cpp
@@ -216,9 +216,14 @@ class WaypointToTarget : public rclcpp::Node
         metrics_arr.data[common_wpt::CUR_LAT_DIST_ABS] = smallest_curr_distance;
         metrics_arr.data[common_wpt::CUR_WAYPOINT_ID] = closest_waypoint;
         metrics_arr.data[common_wpt::AVG_LAT_DISTANCE] = average_distance;
-
-        metrics_arr.data[common_wpt::CUR_LAT_DIST_SIGNED] = msg.poses[closest_waypoint].position.x; 
-
+        float lat_dist = msg.poses[closest_waypoint].position.x - current_pose.position.x;
+        if(lat_dist < 0)
+        {
+            metrics_arr.data[common_wpt::CUR_LAT_DIST_SIGNED] = smallest_curr_distance; 
+        }
+        else{
+            metrics_arr.data[common_wpt::CUR_LAT_DIST_SIGNED] = -1.0 * smallest_curr_distance; 
+        }
         // calculate the adaptive lookahead distance
         double lookahead_actual = calcLookahead(speed_msg.data);