Stanley #2 and marker fix: #6

src/stanley_control.cpp

@@ -7,6 +7,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/color_rgba.hpp"
 #include "std_msgs/msg/float32.hpp"
+#include "std_msgs/msg/float32_multi_array.hpp"
 #include "std_msgs/msg/bool.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 #include "visualization_msgs/msg/marker.hpp"
@@ -19,6 +20,8 @@
 #include "tf2/LinearMath/Quaternion.h"
 #include "tf2/LinearMath/Matrix3x3.h"
 
+#include "wayp_plan_tools/common.hpp"
+
 std::string waypoint_topic = "/lexus3/pursuitgoal";
 geometry_msgs::msg::Twist pursuit_vel;
 
@@ -48,6 +51,10 @@ class StanleyControl : public rclcpp::Node
       {
         heading_err_rate = param.as_double();
       }
+      if(param.get_name() == "cross_track_err_rate")
+      {
+        cross_track_err_rate = param.as_double();
+      }
     }
     return result;
   }
@@ -67,6 +74,7 @@ class StanleyControl : public rclcpp::Node
     reinit_pub_ = this->create_publisher<std_msgs::msg::Bool>("/lexus3/control_reinit", 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>("/lexus3/pursuitspeedtarget", 10, std::bind(&StanleyControl::speedCallback, this, _1));
+    sub_m_ = this->create_subscription<std_msgs::msg::Float32MultiArray>("lexus3/metrics_wayp", 10, std::bind(&StanleyControl::metricsCallback, this, _1));
     timer_ = this->create_wall_timer(50ms, std::bind(&StanleyControl::timerLoop, this));
     callback_handle_ = this->add_on_set_parameters_callback(std::bind(&StanleyControl::parametersCallback, this, std::placeholders::_1));
     std::this_thread::sleep_for(600ms);
@@ -84,8 +92,10 @@ class StanleyControl : public rclcpp::Node
     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;
     // 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;
   }
 
@@ -106,6 +116,12 @@ class StanleyControl : public rclcpp::Node
     m.getRPY(goal_roll, goal_pitch, goal_yaw);
     pursuit_vel.angular.z = calcStanley(msg.poses[0].position.x, msg.poses[0].position.y, goal_yaw);
   }
+
+  void metricsCallback(const std_msgs::msg::Float32MultiArray &metrics_arr)
+  {
+    cur_cross_track_err = metrics_arr.data[common_wpt::CUR_LAT_DIST_SIGNED];
+  }
+
   void timerLoop()
   {
     // RCLCPP_INFO_STREAM(this->get_logger(), "timer");
@@ -121,13 +137,16 @@ class StanleyControl : public rclcpp::Node
 
   rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr sub_w_;
   rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_s_;
+  rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr sub_m_;
   // parameters
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr goal_pub_;
   rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr reinit_pub_;
   rclcpp::TimerBase::SharedPtr timer_;
   float wheelbase = 2.789; // Lexus3
+  float cur_cross_track_err = 0.0;
   OnSetParametersCallbackHandle::SharedPtr callback_handle_;
   float heading_err_rate = 1.0; // TODO: tune
+  float cross_track_err_rate = 1.0; // TODO: tune
 };
 
 int main(int argc, char **argv)

src/waypoint_loader.cpp

@@ -34,6 +34,7 @@
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose_array.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
 
 #include "tf2/LinearMath/Quaternion.h"
 #include "tf2/LinearMath/Matrix3x3.h"
@@ -43,14 +44,32 @@ using std::placeholders::_1;
 
 class WaypointLoader : public rclcpp::Node
 {
+  rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> &parameters)
+  {
+    rcl_interfaces::msg::SetParametersResult result;
+    result.successful = true;
+    result.reason = "success";
+    for (const auto &param : parameters)
+    {
+      RCLCPP_INFO_STREAM(this->get_logger(), "Param update: " << param.get_name().c_str() << ": " << param.value_to_string().c_str());
+      if (param.get_name() == "speed_marker_unit")
+      {
+        speed_marker_unit = param.as_string();
+      }
+    }
+    return result;
+  }
+
 public:
   WaypointLoader() : Node("waypoint_loader_node")
   {
     this->declare_parameter<std::string>("file_dir", "");
     this->declare_parameter<std::string>("file_name", "");
     this->declare_parameter<std::string>("waypoint_topic", "");
+    this->declare_parameter<std::string>("speed_marker_unit", "kmph");
     this->get_parameter("file_dir", file_dir);
     this->get_parameter("file_name", file_name);
+    this->get_parameter("speed_marker_unit", speed_marker_unit);
     multi_file_path_.clear();
     /*
     if (file_name.empty())
@@ -67,6 +86,7 @@ class WaypointLoader : public rclcpp::Node
     speed_pub_ = this->create_publisher<std_msgs::msg::Float32MultiArray>("lexus3/waypointarray_speeds", 1);
     mark_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("lexus3/waypointarray_marker", 1);
     timer_ = this->create_wall_timer(500ms, std::bind(&WaypointLoader::timer_callback, this));
+    callback_handle_ = this->add_on_set_parameters_callback(std::bind(&WaypointLoader::parametersCallback, this, std::placeholders::_1));
     RCLCPP_INFO_STREAM(this->get_logger(), "Loaded file: " << file_dir << "/" << file_name << " ");
   }
 
@@ -267,7 +287,18 @@ class WaypointLoader : public rclcpp::Node
         text_elem.color.b = 0.537;
         text_elem.color.a = 1.0;
         std::stringstream stream;
-        stream << std::fixed << std::setprecision(1) << speeds[id] * 3.6 << " km/h";
+        if (speed_marker_unit == "mps") // Meter per second (m/s)
+        {
+          stream << std::fixed << std::setprecision(1) << speeds[id] << " m/s";
+        }
+        else if (speed_marker_unit == "mph") // Miles per hour (mph)
+        {
+          stream << std::fixed << std::setprecision(1) << speeds[id] * 2.23694 << " mph";
+        }
+        else // Kilometer per hour (km/h)
+        {
+          stream << std::fixed << std::setprecision(1) << speeds[id] * 3.6 << " km/h";
+        }
         text_elem.text = stream.str();
         text_elem.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
         text_elem.action = visualization_msgs::msg::Marker::ADD;
@@ -285,7 +316,7 @@ class WaypointLoader : public rclcpp::Node
 
   void timer_callback()
   {
-    //RCLCPP_INFO(this->get_logger(), "timer");
+    // RCLCPP_INFO(this->get_logger(), "timer");
     for (const auto &el : multi_file_path_)
     {
       pubLaneWaypoint(el);
@@ -295,8 +326,9 @@ class WaypointLoader : public rclcpp::Node
   rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr lane_pub_;
   rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr speed_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr mark_pub_;
+  OnSetParametersCallbackHandle::SharedPtr callback_handle_;
   std::vector<std::string> multi_file_path_;
-  std::string file_dir, file_name;
+  std::string file_dir, file_name, speed_marker_unit;
   double interval_ = 0.4;
   bool topic_based_saving; // topic or transform (tf) based saving
   geometry_msgs::msg::PoseArray lane_array;

src/waypoint_to_target.cpp

@@ -217,6 +217,8 @@ class WaypointToTarget : public rclcpp::Node
         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; 
+
         // calculate the adaptive lookahead distance
         double lookahead_actual = calcLookahead(speed_msg.data);