diff --git a/README.md b/README.md index 09f8afc..24d00bb 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# Veddar VESC Interface +# Ray-Quasar: Veddar VESC Interface ![ROS2 CI Workflow](https://github.com/f1tenth/vesc/workflows/ROS2%20CI%20Workflow/badge.svg) @@ -6,6 +6,11 @@ Packages to interface with Veddar VESC motor controllers. See https://vesc-proje This is a ROS2 implementation of the ROS1 driver using the new serial driver located in [transport drivers](https://github.com/ros-drivers/transport_drivers). +## Ackermann to VESC Node has been reconfigured to publish braking messages. +This functionality existed in the ROS1 version of this package but was not included in the port to ROS2. I have added it back in, and improved it somewhat. + +If you are an F1Tenth/Roboracer team using Traxxas' Velineon 3351R 3500 BLDC motor [this motor configuration XML](https://github.com/ray-quasar/vesc/blob/main/velineon3500_HFI.xml) will get you HFI for smoother starts as well. + ## How to test 1. Clone this repository and [transport drivers](https://github.com/ros-drivers/transport_drivers) into `src`. diff --git a/velineon3500_HFI.xml b/velineon3500_HFI.xml new file mode 100644 index 0000000..326f7ad --- /dev/null +++ b/velineon3500_HFI.xml @@ -0,0 +1,217 @@ + + + 3 + 1 + 0 + 2 + 0 + 99 + -71 + 99 + -60 + 1 + 0.005 + 150 + -100000 + 100000 + 0.8 + 300 + 1500 + 8 + 57 + 10.2 + 9 + 1000 + 1100 + 0 + 85 + 100 + 85 + 100 + 0.15 + 0.005 + 0.95 + 1.5e+06 + -1.5e+06 + 1 + 1 + 1 + 150 + 1100 + 10 + 62 + 0.8 + 80000 + 600 + -1 + 1 + 3 + 2 + 5 + 6 + 4 + -1 + 2000 + 0.00264233 + 6.61179 + 30000 + 0.12 + 0 + 180 + 7 + 3 + 2000 + 30000 + 2.64233e-06 + 2.69788e-07 + 0.00661179 + 0.000707035 + 2.00041e+09 + 0.05 + -1 + 50 + 1000 + 1 + 2500 + 1400 + 0 + 0.9 + 0.2 + 0.1 + 0 + 0.1 + 0.05 + 0 + -1 + 255 + 255 + 255 + 255 + 255 + 255 + 255 + 255 + 500 + 2500 + 4000 + 0 + 0 + 0 + 0 + 0 + 25 + 0.1 + 0 + 3 + 10 + 6 + 8 + 0.3 + 0.15 + 0 + 3000 + 5 + 0.001 + 1 + 1 + 2048.78 + 2049.13 + 2048.43 + -0.0004 + 0.0001 + 0.0002 + 0 + 0 + 0 + 1 + 1 + 4000 + 0 + 0 + 0.9 + 0.02 + 0.2 + 0 + 0 + 5 + 0.002 + 0.0015 + 0.0002 + 0.2 + 900 + 1 + 25000 + 0 + 0.025 + 0 + 0 + 0 + 0.00035 + 0.2 + 1 + 0 + 0.01 + 0.05 + 0.0046 + 0.04 + 500 + 0.02 + 0.5 + 8192 + 1 + 1 + 1.65 + 1.65 + 0.5 + 0 + 0 + 0 + 0 + 16 + 3000 + 35000 + 25000 + 3380 + 0 + 0 + 0.61 + 10000 + 25 + 3 + 45 + 2 + 1.459 + 0.11 + 0 + 3 + 5 + 1 + Traxxas + Velineon 3500 + 248 + 0 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; font-size:12pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">A motor description can be edited here.</p></body></html> + 0 + 0 + 0 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; font-size:12pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Some comments about the motor quality. Images can be added as well.</p></body></html> + 1 + 3 + 45 + 65 + 0.05 + 0 + 2.9 + 2.5 + 4.2 + 4.3 + 0 + diff --git a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp index b646c36..676e5ae 100644 --- a/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp +++ b/vesc_ackermann/include/vesc_ackermann/ackermann_to_vesc.hpp @@ -32,6 +32,7 @@ #define VESC_ACKERMANN__ACKERMANN_TO_VESC_HPP_ #include +#include #include #include @@ -39,6 +40,7 @@ namespace vesc_ackermann { using ackermann_msgs::msg::AckermannDriveStamped; +using nav_msgs::msg::Odometry; using std_msgs::msg::Float64; class AckermannToVesc : public rclcpp::Node @@ -48,19 +50,35 @@ class AckermannToVesc : public rclcpp::Node private: // ROS parameters + u_int8_t operation_mode_; + // bool previous_mode_speed_ = true; // conversion gain and offset double speed_to_erpm_gain_, speed_to_erpm_offset_; + double speed_to_braking_gain_, speed_to_braking_center_, speed_to_braking_max_, speed_to_braking_min_; double steering_to_servo_gain_, steering_to_servo_offset_; + double current_vel_, brake_deadzone_; + double accel_to_current_gain_, accel_to_brake_gain_; + + enum operation_modes { + ACCEL_TO_CURRENT, + VEL_TO_CURRENT, + VEL_TO_ERPM + }; /** @todo consider also providing an interpolated look-up table conversion */ // ROS services rclcpp::Publisher::SharedPtr erpm_pub_; rclcpp::Publisher::SharedPtr servo_pub_; + rclcpp::Publisher::SharedPtr brake_pub_; + rclcpp::Publisher::SharedPtr current_pub_; + + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr ackermann_sub_; // ROS callbacks void ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd); + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg); }; } // namespace vesc_ackermann diff --git a/vesc_ackermann/src/ackermann_to_vesc.cpp b/vesc_ackermann/src/ackermann_to_vesc.cpp index 12a69a0..2bc42b1 100644 --- a/vesc_ackermann/src/ackermann_to_vesc.cpp +++ b/vesc_ackermann/src/ackermann_to_vesc.cpp @@ -31,7 +31,7 @@ #include "vesc_ackermann/ackermann_to_vesc.hpp" #include -#include +#include #include #include @@ -40,53 +40,290 @@ namespace vesc_ackermann { -using ackermann_msgs::msg::AckermannDriveStamped; -using std::placeholders::_1; -using std_msgs::msg::Float64; + using ackermann_msgs::msg::AckermannDriveStamped; + using nav_msgs::msg::Odometry; + using std::placeholders::_1; + using std_msgs::msg::Float64; -AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions & options) -: Node("ackermann_to_vesc_node", options) -{ - // declare parameters - declare_parameter("speed_to_erpm_gain", 0.0); - declare_parameter("speed_to_erpm_offset", 0.0); - declare_parameter("steering_angle_to_servo_gain", 0.0); - declare_parameter("steering_angle_to_servo_offset", 0.0); - - // get conversion parameters - speed_to_erpm_gain_ = get_parameter("speed_to_erpm_gain").get_value(); - speed_to_erpm_offset_ = get_parameter("speed_to_erpm_offset").get_value(); - steering_to_servo_gain_ = get_parameter("steering_angle_to_servo_gain").get_value(); - steering_to_servo_offset_ = get_parameter("steering_angle_to_servo_offset").get_value(); - - // create publishers to vesc electric-RPM (speed) and servo commands - erpm_pub_ = create_publisher("commands/motor/speed", 10); - servo_pub_ = create_publisher("commands/servo/position", 10); - - // subscribe to ackermann topic - ackermann_sub_ = create_subscription( - "ackermann_cmd", 10, std::bind(&AckermannToVesc::ackermannCmdCallback, this, _1)); -} - -void AckermannToVesc::ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd) -{ - // calc vesc electric RPM (speed) - Float64 erpm_msg; - erpm_msg.data = speed_to_erpm_gain_ * cmd->drive.speed + speed_to_erpm_offset_; + AckermannToVesc::AckermannToVesc(const rclcpp::NodeOptions &options) + : Node("ackermann_to_vesc_node", options) + { + // Declare parameters, initialized with default 0 + // These four are what we started with: + declare_parameter("speed_to_erpm_gain", 0.0); + declare_parameter("speed_to_erpm_offset", 0.0); + declare_parameter("steering_angle_to_servo_gain", 0.0); + declare_parameter("steering_angle_to_servo_offset", 0.0); + + // I added this one (originally just a regular variable) as part of the first braking test: + // Renaming to brake_deadzone + declare_parameter("brake_deadzone", 0.1); + + // Adding braking parameters for VEL_TO_ERPM mode + declare_parameter("speed_to_braking_gain", 0.0); + declare_parameter("speed_to_braking_center", 0.0); + declare_parameter("speed_to_braking_max", 20000.0); + declare_parameter("speed_to_braking_min", 0.0); + + // I am adding these two in as part of the conversion to acceleration based control: + declare_parameter("accel_to_current_gain", 0.0); + declare_parameter("accel_to_brake_gain", 0.0); + + // Get conversion parameters from config file, in our case 'vesc.yaml' + // Originals: + speed_to_erpm_gain_ = get_parameter("speed_to_erpm_gain").get_value(); + speed_to_erpm_offset_ = get_parameter("speed_to_erpm_offset").get_value(); + steering_to_servo_gain_ = get_parameter("steering_angle_to_servo_gain").get_value(); + steering_to_servo_offset_ = get_parameter("steering_angle_to_servo_offset").get_value(); + + // Braking Test: + brake_deadzone_ = get_parameter("brake_deadzone").get_value(); + + // Braking parameters for VEL_TO_ERPM mode + speed_to_braking_gain_ = get_parameter("speed_to_braking_gain").get_value(); + speed_to_braking_center_ = get_parameter("speed_to_braking_center").get_value(); + speed_to_braking_max_ = get_parameter("speed_to_braking_max").get_value(); + speed_to_braking_min_ = get_parameter("speed_to_braking_min").get_value(); + + // Acceleration Test: + accel_to_current_gain_ = get_parameter("accel_to_current_gain").get_value(); + accel_to_brake_gain_ = get_parameter("accel_to_brake_gain").get_value(); + + // Create publishers to VESC electric-RPM (speed) and servo commands + // The ERPM publisher should only be used if the acceleration parameter is 0 + erpm_pub_ = create_publisher("commands/motor/speed", 10); + servo_pub_ = create_publisher("commands/servo/position", 10); + + // Create brake publisher + brake_pub_ = create_publisher("commands/motor/brake", 10); + // Initialize current velocity storage variable + current_vel_ = 0.0; + + // Create current publisher + current_pub_ = create_publisher("commands/motor/current", 10); + + // Subscribe to Ackermann topic + // This is what supplies the input information for the rest of the code. + // I don't care to change the disparityExtender code, so we'll just work with the speed commands therein. + ackermann_sub_ = create_subscription( + "ackermann_cmd", 10, std::bind(&AckermannToVesc::ackermannCmdCallback, this, _1)); + + // Subscribe to odom topic + // Because we are just listening to the speed commands, we need a tracker of the existing velocity + odom_sub_ = create_subscription( + "odom", 10, std::bind(&AckermannToVesc::odomCallback, this, _1)); + } + + void AckermannToVesc::ackermannCmdCallback(const AckermannDriveStamped::SharedPtr cmd) + { + // Initialize all messages at the start + Float64 servo_msg; + Float64 brake_msg; + Float64 current_msg; + Float64 erpm_msg; + bool publish_brake = false; + bool publish_ERPM = false; + + // Zero-initialize message data + brake_msg.data = 0.0; + current_msg.data = 0.0; + erpm_msg.data = 0.0; + + // Calculate steering angle (servo) + servo_msg.data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; + + // Calculate BLDC command + // Case 1: We are supplied the acceleration-to-current gain and acceleration-to-brake gain + if (accel_to_current_gain_ != 0 && accel_to_brake_gain_ != 0) + { + bool is_positive_accel = true; + // 1.1 We have an incoming acceleration command. + if (cmd->drive.acceleration != 0) + { + operation_mode_ = ACCEL_TO_CURRENT; // For deciding which message to publish + if (cmd->drive.acceleration < 0) + { + // Apply controlled braking + brake_msg.data = accel_to_brake_gain_ * cmd->drive.acceleration; + // is_positive_accel = false; + } else + { + // Apply motor current (throttle) + current_msg.data = accel_to_current_gain_ * cmd->drive.acceleration; + } + // previous_mode_speed_ = false; + } + // 1.2 We do not have an incoming acceleration command. We have an incoming velocity command. + else if (cmd->drive.speed != 0 || operation_mode_ == VEL_TO_CURRENT) { + // Calculate the required acceleration: + // TODO: Calculate dt + operation_mode_ = VEL_TO_CURRENT; // For deciding which message to publish + double commanded_vel = cmd->drive.speed; + double acceleration = 10 * (commanded_vel - current_vel_); // . / 0.1; // Desired acceleration + if (acceleration > 0) + { + // Apply motor current (throttle) + current_msg.data = acceleration * accel_to_current_gain_; + } else + { + // Apply controlled braking + brake_msg.data = -acceleration * accel_to_brake_gain_; + // is_positive_accel = false; + } + } + } + // Case 2: If the acceleration-to-current gain and acceleration-to-brake gain is 0, then we operate entirely with the velocity message + else if (accel_to_current_gain_ == 0 && accel_to_brake_gain_ == 0) + { + operation_mode_ = VEL_TO_ERPM; // For deciding which message to publish + double commanded_vel = cmd->drive.speed; + + // Case A: We have a postive commanded velocity: + if (commanded_vel >= 0) + { + // I. The commanded velocity is greater than the current velocity + if (commanded_vel >= current_vel_) + { + // If the current velocity is negative apply the brake + if (current_vel_ < 0) + { + // Calculate the brake value + brake_msg.data = ( + (speed_to_braking_max_) + / (1 + exp(- speed_to_braking_gain_ * (abs(current_vel_ - commanded_vel) - speed_to_braking_center_))) + ); + publish_brake = true; + } + // If the current velocity is zero we need to go slow to get a rotor position reading before we can accelerate + else if (current_vel_ < 1 && commanded_vel > 1) + { + // Send a (0.5 m/s) ERPM message + // Calculate the ERPM using the speed-to-ERPM gain and offset: + erpm_msg.data = speed_to_erpm_gain_ * (current_vel_ + 0.4) + speed_to_erpm_offset_; + // Hopefully the compiler will pick this up and pre-evaluate + publish_ERPM = true; + } + // The current velocity is greater than zero + else + { + // Calculate the ERPM using the speed-to-ERPM gain and offset: + erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; + publish_ERPM = true; + } + } + // II. The commanded velocity is less than the current velocity + else if (commanded_vel < current_vel_) + { + // Apply the BRAKE + brake_msg.data = ( + (speed_to_braking_max_) + / (1 + exp(- speed_to_braking_gain_ * (abs(current_vel_ - commanded_vel) - speed_to_braking_center_))) + ); + // is_positive_accel = false; + publish_brake = true; + } + } + + // Case B: We have a negative commanded velocity: + else if (commanded_vel < 0) + { + // I. The commanded velocity is less (faster) than the current velocity + if (commanded_vel <= current_vel_) + { + // If the current velocity is positive + if (current_vel_ > 0) + { + // Apply the brake + brake_msg.data = ( + (speed_to_braking_max_) + / (1 + exp(- speed_to_braking_gain_ * (abs(current_vel_ - commanded_vel) - speed_to_braking_center_))) + ); + publish_brake = true; + } + // If the current velocity is zero we need to go slow to get a rotor position reading + else if (current_vel_ == 0 && commanded_vel < -0.5) + { + // Send a (-0.5 m/s) ERPM message + erpm_msg.data = speed_to_erpm_gain_ * -0.5 + speed_to_erpm_offset_; + // Hopefully the compiler will pick this up and pre-evaluate + publish_ERPM = true; + } + // The current velocity is negative + else + { + // Calculate the ERPM using the speed-to-ERPM gain and offset: + erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; + publish_ERPM = true; + } + } + // II. The commanded velocity is greater (slower) than the current velocity + else if (commanded_vel > current_vel_) + { + // Apply the BRAKE + // Apply the BRAKE + brake_msg.data = ( + (speed_to_braking_max_) + / (1 + exp(- speed_to_braking_gain_ * (abs(current_vel_ - commanded_vel) - speed_to_braking_center_))) + ); + // is_positive_accel = false; + publish_brake = true; + } + } + } + // ----- OLD CODE ------ + + // double vel_diff = current_vel_ - commanded_vel; + // // 2.1 The commanded velocity has increased: + // if (vel_diff < 0) + // { + // // Calculate the ERPM using the speed-to-ERPM gain and offset: + // erpm_msg.data = speed_to_erpm_gain_ * commanded_vel + speed_to_erpm_offset_; + // } + // // 2.2 The commanded velocity has decreased: + // if (vel_diff > 0) + // { + // // Calculate the brake value: + // // brake_msg.data = (vel_diff) * speed_to_braking_gain_ + speed_to_braking_center_; + // brake_msg.data = ( + // (speed_to_braking_max_) + // / (1 + exp(- speed_to_braking_gain_ * (vel_diff - speed_to_braking_center_))) + // ); + // // brake_msg.data = std::clamp(brake_msg.data, 0.0, 20000.0); + // is_positive_accel = false; + // } + // } + + // Publish motor commands: + if (rclcpp::ok()) { + if (publish_brake) { + if (brake_msg.data != 0.0) { // Only publish if we actually set a brake value + brake_pub_->publish(brake_msg); + } + } else if (publish_ERPM) { + if (operation_mode_ == ACCEL_TO_CURRENT || operation_mode_ == VEL_TO_CURRENT) { + if (current_msg.data != 0.0) { + current_pub_->publish(current_msg); + } + } else if (operation_mode_ == VEL_TO_ERPM) { + if (erpm_msg.data != 0.0) { + erpm_pub_->publish(erpm_msg); + } + } + } + servo_pub_->publish(servo_msg); + } + } - // calc steering angle (servo) - Float64 servo_msg; - servo_msg.data = steering_to_servo_gain_ * cmd->drive.steering_angle + steering_to_servo_offset_; - // publish - if (rclcpp::ok()) { - erpm_pub_->publish(erpm_msg); - servo_pub_->publish(servo_msg); - } -} + void AckermannToVesc::odomCallback(const nav_msgs::msg::Odometry::SharedPtr odom_msg) + { + // Everytime there is a new publish to /odom, we update the current velocity + current_vel_ = odom_msg->twist.twist.linear.x; + // RCLCPP_INFO(get_logger(), "Current velocity: %f", current_vel_); + } -} // namespace vesc_ackermann +} // namespace vesc_ackermann -#include "rclcpp_components/register_node_macro.hpp" // NOLINT +#include "rclcpp_components/register_node_macro.hpp" // NOLINT RCLCPP_COMPONENTS_REGISTER_NODE(vesc_ackermann::AckermannToVesc) diff --git a/vesc_ackermann/src/vesc_to_odom.cpp b/vesc_ackermann/src/vesc_to_odom.cpp index 122b5b9..bf18c60 100644 --- a/vesc_ackermann/src/vesc_to_odom.cpp +++ b/vesc_ackermann/src/vesc_to_odom.cpp @@ -104,7 +104,7 @@ void VescToOdom::vescStateCallback(const VescStateStamped::SharedPtr state) } // convert to engineering units - double current_speed = (state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; + double current_speed = (state->state.speed - speed_to_erpm_offset_) / speed_to_erpm_gain_; if (std::fabs(current_speed) < 0.05) { current_speed = 0.0; }