Add more sophisticated drive control methods to WheelSpeedNode

Author: epw1624

src/drive_control/include/wheel_speed.h

@@ -8,6 +8,11 @@
 #include <vector>
 #include "rclcpp/qos.hpp"  // Include QoS header
 
+struct WheelSpeeds {
+  std::vector<double> left_wheel_speeds;
+  std::vector<double> right_wheel_speeds; 
+};
+
 class WheelSpeedNode : public rclcpp::Node {
 public:
     WheelSpeedNode();
@@ -20,6 +25,10 @@ class WheelSpeedNode : public rclcpp::Node {
     rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr right_wheel_pub_;
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
 
+    // Functions for converting cmd_vel_ messages to wheel speeds, for different drive control behavior
+    WheelSpeeds schmittTrigger(double linear, double angular); // Schmitt trigger/hysteresis
+    WheelSpeeds ackermann(double linear, double angular); // Electronic Ackermann Steering
+
     static constexpr double WHEEL_RADIUS_METERS = 0.3;  // Wheel radius
     static constexpr double TRACK_WIDTH_METERS = 0.6;   // Distance between left and right wheels
 };

src/drive_control/src/wheel_speed.cpp

@@ -28,29 +28,56 @@ void WheelSpeedNode::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr m
     linear *= wheel_speed_factor;
     angular *= wheel_speed_factor;
 
-    // Create vectors of motor commands that correspond to the calculated speeds for left and right wheels
-    std::vector<double> left_wheel_commands = {
-        -linear + angular,   // Left Front Wheel
-        -linear + angular,   // Left Mid Wheel
-        -linear + angular    // Left Rear Wheel
-    };
+    WheelSpeeds wheel_speeds = schmittTrigger(linear, angular);
+    // std::vector<double> wheel_commands = ackermann(linear, angular);
+
+    // // Create vectors of motor commands that correspond to the calculated speeds for left and right wheels
+    // std::vector<double> left_wheel_commands = {
+    //     -linear + angular,   // Left Front Wheel
+    //     -linear + angular,   // Left Mid Wheel
+    //     -linear + angular    // Left Rear Wheel
+    // };
 
-    std::vector<double> right_wheel_commands = {
-        linear + angular,  // Right Front Wheel
-        linear + angular,  // Right Mid Wheel
-        linear + angular   // Right Rear Wheel
-    };
+    // std::vector<double> right_wheel_commands = {
+    //     linear + angular,  // Right Front Wheel
+    //     linear + angular,  // Right Mid Wheel
+    //     linear + angular   // Right Rear Wheel
+    // };
 
     // Publish the separate left and right wheel speed messages
     std_msgs::msg::Float64MultiArray left_msg;
-    left_msg.data = left_wheel_commands;  // Set left wheel commands data
+    left_msg.data = wheel_speeds.left_wheel_speeds;  // Set left wheel commands data
     left_wheel_pub_->publish(left_msg);   // Publish to "/left_wheel_speeds"
 
     std_msgs::msg::Float64MultiArray right_msg;
-    right_msg.data = right_wheel_commands;  // Set right wheel commands data
+    right_msg.data = wheel_speeds.right_wheel_speeds;  // Set right wheel commands data
     right_wheel_pub_->publish(right_msg);   // Publish to "/right_wheel_speeds"
 }
 
+WheelSpeeds WheelSpeedNode::schmittTrigger(double linear, double angular) {
+    // Tank drive as placeholder
+    double l = -linear + angular;
+    double r = linear + angular;
+
+    WheelSpeeds wheelSpeeds;
+    wheelSpeeds.left_wheel_speeds = { l, l, l };
+    wheelSpeeds.right_wheel_speeds = { r, r, r };
+
+    return wheelSpeeds;
+}
+
+WheelSpeeds WheelSpeedNode::ackermann(double linear, double angular) {
+    // Tank drive as placeholder
+    double l = -linear + angular;
+    double r = linear + angular;
+
+    WheelSpeeds wheelSpeeds;
+    wheelSpeeds.left_wheel_speeds = { l, l, l };
+    wheelSpeeds.right_wheel_speeds = { r, r, r };
+
+    return wheelSpeeds;
+}
+
 // Main function to initialize the ROS2 node and start processing
 int main(int argc, char *argv[]) {
     rclcpp::init(argc, argv);  // Initialize the ROS2 system