Stanley cross-track error based on angle difference instead of distance

#1
jkk-research · Sep 30, 2024 · aeb6feb · aeb6feb
1 parent 509e7b9
commit aeb6feb
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 18 deletions.
diff --git a/README.md b/README.md
@@ -73,6 +73,7 @@ It also provides a `/metrics_wayp` array topic with the following  elements:
 |`[5]` | target waypoint ID | `TRG_WAYPOINT_ID`
 |`[6]` | target waypoint longitudinal distance (similar to lookahed distance, but at a waypoint) | `TRG_WAY_LON_DIST`
 |`[7]` | actual lookahead distance | `ACT_LOOK_DIST`
+|`[8]` | current cross-track error | `CUR_CROSS_TRACK`
 
 
 <img src="csv/lookahead01.svg" width=60% />

diff --git a/include/wayp_plan_tools/common.hpp b/include/wayp_plan_tools/common.hpp
@@ -19,6 +19,7 @@ namespace common_wpt
         TRG_WAYPOINT_ID,         // target waypoint ID
         TRG_WAY_LON_DIST,        // target waypoint longitudinal distance
         ACT_LOOK_DIST,           // actual lookahead distance        
+        CUR_CROSS_TRACK,         // current cross-track error
         NOT_USED_YET
     };
 } // namespace common_wpt

diff --git a/src/stanley_control.cpp b/src/stanley_control.cpp
@@ -48,13 +48,13 @@ class StanleyControl : public rclcpp::Node
       {
         wheelbase = param.as_double();
       }
-      if (param_name == "heading_err_rate")
+      if (param_name == "heading_err_gain")
       {
-        heading_err_rate = param.as_double();
+        heading_err_gain = param.as_double();
       }
-      if (param_name == "cross_track_err_rate")
+      if (param_name == "cross_track_err_gain")
       {
-        cross_track_err_rate = param.as_double();
+        cross_track_err_gain = param.as_double();
       }   
     }
     return result;
@@ -74,18 +74,21 @@ class StanleyControl : public rclcpp::Node
     this->declare_parameter<std::string>("waypoint_topic", "");
     this->declare_parameter<std::string>("cmd_topic", cmd_topic);
     this->declare_parameter<float>("wheelbase", wheelbase);
-    this->declare_parameter<float>("heading_err_rate", heading_err_rate, descriptor_slider);
-    this->declare_parameter<float>("pursuit_rate", pursuit_rate, descriptor_slider);
-    this->declare_parameter<float>("cross_track_err_rate", cross_track_err_rate, descriptor_slider);
+    this->declare_parameter<float>("heading_err_gain", heading_err_gain, descriptor_slider);
+    this->declare_parameter<float>("pursuit_gain", pursuit_gain, descriptor_slider);
+    this->declare_parameter<float>("cross_track_err_gain", cross_track_err_gain, descriptor_slider);
     this->get_parameter("waypoint_topic", waypoint_topic);
     this->get_parameter("cmd_topic", cmd_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);
-    this->get_parameter("pursuit_rate", pursuit_rate);
+    this->get_parameter("heading_err_gain", heading_err_gain);
+    this->get_parameter("cross_track_err_gain", cross_track_err_gain);
+    this->get_parameter("pursuit_gain", pursuit_gain);
 
     goal_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(cmd_topic, 10);
     reinit_pub_ = this->create_publisher<std_msgs::msg::Bool>("control_reinit", 10);
+    deb_c_pub = this->create_publisher<std_msgs::msg::Float32>("debug_cross_track_err", 10);
+    deb_h_pub = this->create_publisher<std_msgs::msg::Float32>("debug_heading_err", 10);
+    deb_p_pub = this->create_publisher<std_msgs::msg::Float32>("debug_pursuit_err", 10);
     sub_w_ = this->create_subscription<geometry_msgs::msg::PoseArray>(waypoint_topic, 10, std::bind(&StanleyControl::waypointCallback, this, _1));
     sub_s_ = this->create_subscription<std_msgs::msg::Float32>("pursuitspeedtarget", 10, std::bind(&StanleyControl::speedCallback, this, _1));
     sub_m_ = this->create_subscription<std_msgs::msg::Float32MultiArray>("metrics_wayp", 10, std::bind(&StanleyControl::metricsCallback, this, _1));
@@ -106,11 +109,21 @@ class StanleyControl : public rclcpp::Node
     float lookahead_distance = sqrt(pow(goal_x, 2) + pow(goal_y, 2));
     // cross-track error
     float purs_steering_angle = atan2(2.0 * wheelbase * sin(alpha), lookahead_distance);
-    float cross_track_error = cur_cross_track_err * cross_track_err_rate;
+    float pursuit_error = pursuit_gain * purs_steering_angle;
+    float cross_track_error = cur_cross_track_err * cross_track_err_gain;
     // heading error
-    float heading_error = goal_yaw * heading_err_rate;
-    RCLCPP_INFO_STREAM(this->get_logger(), "purs: " << std::fixed << std::setprecision(2) << pursuit_rate * purs_steering_angle << ", crosstrackE: " << cross_track_error << ", headingE: " << heading_error);
-    return pursuit_rate * purs_steering_angle + cross_track_error + heading_error;
+    float heading_error = goal_yaw * heading_err_gain;
+    RCLCPP_INFO_STREAM(this->get_logger(), "purs: " << std::fixed << std::setprecision(2) << pursuit_gain * purs_steering_angle << ", crosstrackE: " << cross_track_error << ", headingE: " << heading_error);
+
+    std_msgs::msg::Float32 deb_c, deb_h, deb_p;
+    deb_c.data = cross_track_error;
+    deb_h.data = heading_error;
+    deb_p.data = pursuit_error;
+    deb_c_pub->publish(deb_c);
+    deb_h_pub->publish(deb_h);
+    deb_p_pub->publish(deb_p);
+
+    return pursuit_error + cross_track_error + heading_error;
   }
 
   void speedCallback(const std_msgs::msg::Float32 &msg) const
@@ -133,7 +146,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_err = metrics_arr.data[common_wpt::CUR_CROSS_TRACK];
     // cur_cross_track_abs = metrics_arr.data[common_wpt::CUR_LAT_DIST_ABS];
   }
 
@@ -156,15 +169,16 @@ class StanleyControl : public rclcpp::Node
   // parameters
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr goal_pub_;
   rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr reinit_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr deb_c_pub, deb_h_pub, deb_p_pub;
   rclcpp::TimerBase::SharedPtr timer_;
   float wheelbase = 2.789;
   std::string cmd_topic;
   float cur_cross_track_err = 0.0;
   float cur_cross_track_abs = 0.0;
   OnSetParametersCallbackHandle::SharedPtr callback_handle_;
-  float heading_err_rate = 0.05;     // TODO: tune
-  float cross_track_err_rate = 0.05; // TODO: tune
-  float pursuit_rate = 0.9;          // TODO: tune
+  float heading_err_gain = 1.00;     // TODO: tune
+  float cross_track_err_gain = 1.00; // TODO: tune
+  float pursuit_gain = 0.5;          // TODO: tune
 };
 
 int main(int argc, char **argv)

diff --git a/src/waypoint_to_target.cpp b/src/waypoint_to_target.cpp
@@ -305,10 +305,24 @@ class WaypointToTarget : public rclcpp::Node
             }
         }
         average_distance_count += 1;
+
+        // calculate the current cross-track error (orientation diff of the closest waypoint and current pose)
+        tf2::Quaternion q_closest(msg.poses[closest_waypoint].orientation.x, msg.poses[closest_waypoint].orientation.y, msg.poses[closest_waypoint].orientation.z, msg.poses[closest_waypoint].orientation.w);
+        tf2::Matrix3x3 m_closest(q_closest);
+        double closest_roll, closest_pitch, closest_yaw;
+        m_closest.getRPY(closest_roll, closest_pitch, closest_yaw);
+        // calculate the current yaw of the vehicle
+        tf2::Quaternion q_curr(current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w);
+        tf2::Matrix3x3 m_curr(q_curr);
+        double current_roll, current_pitch, current_yaw;
+        double cross_yaw_diff = closest_yaw - current_yaw;
+
         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::CUR_CROSS_TRACK] = cross_yaw_diff;
         metrics_arr.data[common_wpt::AVG_LAT_DISTANCE] = average_distance;
         metrics_arr.data[common_wpt::MAX_LAT_DISTANCE] = maximum_distance;
+
         // calculate the adaptive lookahead distance
         double lookahead_actual = calcLookahead(speed_msg.data);
         metrics_arr.data[common_wpt::ACT_LOOK_DIST] = lookahead_actual;