From 353e90c3b44da9abeaec392bbb7656c6cf4b44d7 Mon Sep 17 00:00:00 2001 From: Ilia <90713890+iliabaranov@users.noreply.github.com> Date: Thu, 7 Oct 2021 13:26:42 -0700 Subject: [PATCH 1/4] Splitting wheel_radius into front_wheel_radius and rear_wheel_radius, ensuring the two sets of wheels can be different sizes --- .../ackermann_steering_controller.h | 6 ++- .../ackermann_steering_controller/odometry.h | 8 ++-- .../src/ackermann_steering_controller.cpp | 43 +++++++++++++------ .../src/odometry.cpp | 10 +++-- 4 files changed, 45 insertions(+), 22 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.h b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.h index 7894f2dbe..796538f7a 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.h +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.h @@ -133,7 +133,8 @@ namespace ackermann_steering_controller{ double wheel_separation_h_; /// Wheel radius (assuming it's the same for the left and right wheels): - double wheel_radius_; + double front_wheel_radius_; + double rear_wheel_radius_; /// Wheel separation and radius calibration multipliers: double wheel_separation_h_multiplier_; @@ -189,7 +190,8 @@ namespace ackermann_steering_controller{ const std::string rear_wheel_name, const std::string front_steer_name, bool lookup_wheel_separation_h, - bool lookup_wheel_radius); + bool lookup_front_wheel_radius, + bool lookup_rear_wheel_radius); /** * \brief Sets the odometry publishing fields diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.h b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.h index 10bb89fd6..53d54a208 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.h +++ b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.h @@ -143,9 +143,10 @@ namespace ackermann_steering_controller /** * \brief Sets the wheel parameters: radius and separation * \param wheel_separation Seperation between left and right wheels [m] - * \param wheel_radius Wheel radius [m] + * \param front_wheel_radius Front (Steer) Wheel radius [m] + * \param rear_wheel_radius Rear (Drive) Wheel radius [m] */ - void setWheelParams(double wheel_reparation_h, double wheel_radius); + void setWheelParams(double wheel_reparation_h, double front_wheel_radius, double rear_wheel_radius); /** * \brief Velocity rolling window size setter @@ -192,7 +193,8 @@ namespace ackermann_steering_controller /// Wheel kinematic parameters [m]: double wheel_separation_h_; - double wheel_radius_; + double front_wheel_radius_; + double rear_wheel_radius_; /// Previous wheel position/state [rad]: double rear_wheel_old_pos_; diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 934b4c49a..81eae955a 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -109,7 +109,8 @@ namespace ackermann_steering_controller{ : open_loop_(false) , command_struct_() , wheel_separation_h_(0.0) - , wheel_radius_(0.0) + , front_wheel_radius_(0.0) + , rear_wheel_radius_(0.0) , wheel_separation_h_multiplier_(1.0) , wheel_radius_multiplier_(1.0) , steer_pos_multiplier_(1.0) @@ -217,13 +218,15 @@ namespace ackermann_steering_controller{ // If either parameter is not available, we need to look up the value in the URDF bool lookup_wheel_separation_h = !controller_nh.getParam("wheel_separation_h", wheel_separation_h_); - bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_); + bool lookup_front_wheel_radius = !controller_nh.getParam("front_wheel_radius", front_wheel_radius_); + bool lookup_rear_wheel_radius = !controller_nh.getParam("rear_wheel_radius", rear_wheel_radius_); if (!setOdomParamsFromUrdf(root_nh, rear_wheel_name, front_steer_name, lookup_wheel_separation_h, - lookup_wheel_radius)) + lookup_front_wheel_radius, + lookup_rear_wheel_radius)) { return false; } @@ -231,11 +234,13 @@ namespace ackermann_steering_controller{ // Regardless of how we got the separation and radius, use them // to set the odometry parameters const double ws_h = wheel_separation_h_multiplier_ * wheel_separation_h_; - const double wr = wheel_radius_multiplier_ * wheel_radius_; - odometry_.setWheelParams(ws_h, wr); + const double fwr = wheel_radius_multiplier_ * front_wheel_radius_; + const double rwr = wheel_radius_multiplier_ * rear_wheel_radius_; + odometry_.setWheelParams(ws_h, fwr, rwr); ROS_INFO_STREAM_NAMED(name_, "Odometry params : wheel separation height " << ws_h - << ", wheel radius " << wr); + << ", front wheel radius " << fwr + << ", rear wheel radius " << rwr); setOdomPubFields(root_nh, controller_nh); @@ -330,7 +335,7 @@ namespace ackermann_steering_controller{ last0_cmd_ = curr_cmd; // Set Command - const double wheel_vel = curr_cmd.lin/wheel_radius_; // omega = linear_vel / radius + const double wheel_vel = curr_cmd.lin/rear_wheel_radius_; // omega = linear_vel / radius, rear wheels are assumed to be drive wheels rear_wheel_joint_.setCommand(wheel_vel); front_steer_joint_.setCommand(curr_cmd.ang); @@ -394,9 +399,10 @@ namespace ackermann_steering_controller{ const std::string rear_wheel_name, const std::string front_steer_name, bool lookup_wheel_separation_h, - bool lookup_wheel_radius) + bool lookup_front_wheel_radius, + bool lookup_rear_wheel_radius) { - if (!(lookup_wheel_separation_h || lookup_wheel_radius)) + if (!(lookup_wheel_separation_h || lookup_front_wheel_radius || lookup_rear_wheel_radius)) { // Short-circuit in case we don't need to look up anything, so we don't have to parse the URDF return true; @@ -451,15 +457,26 @@ namespace ackermann_steering_controller{ ROS_INFO_STREAM("Calculated wheel_separation_h: " << wheel_separation_h_); } - if (lookup_wheel_radius) + if (lookup_rear_wheel_radius) { - // Get wheel radius - if (!getWheelRadius(model->getLink(rear_wheel_joint->child_link_name), wheel_radius_)) + // Get rear wheel radius + if (!getWheelRadius(model->getLink(rear_wheel_joint->child_link_name), rear_wheel_radius_)) { ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << rear_wheel_name << " wheel radius"); return false; } - ROS_INFO_STREAM("Retrieved wheel_radius: " << wheel_radius_); + ROS_INFO_STREAM("Retrieved rear_wheel_radius: " << rear_wheel_radius_); + } + + if (lookup_front_wheel_radius) + { + // Get front wheel radius + if (!getWheelRadius(model->getLink(front_steer_joint->child_link_name), front_wheel_radius_)) + { + ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << front_steer_name << " wheel radius"); + return false; + } + ROS_INFO_STREAM("Retrieved rear_wheel_radius: " << front_wheel_radius_); } return true; diff --git a/ackermann_steering_controller/src/odometry.cpp b/ackermann_steering_controller/src/odometry.cpp index 025b6605f..3575a03b5 100644 --- a/ackermann_steering_controller/src/odometry.cpp +++ b/ackermann_steering_controller/src/odometry.cpp @@ -54,7 +54,8 @@ namespace ackermann_steering_controller , linear_(0.0) , angular_(0.0) , wheel_separation_h_(0.0) - , wheel_radius_(0.0) + , front_wheel_radius_(0.0) + , rear_wheel_radius_(0.0) , rear_wheel_old_pos_(0.0) , velocity_rolling_window_size_(velocity_rolling_window_size) , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size) @@ -73,7 +74,7 @@ namespace ackermann_steering_controller bool Odometry::update(double rear_wheel_pos, double front_steer_pos, const ros::Time &time) { /// Get current wheel joint positions: - const double rear_wheel_cur_pos = rear_wheel_pos * wheel_radius_; + const double rear_wheel_cur_pos = rear_wheel_pos * rear_wheel_radius_; /// Estimate velocity of wheels using old and current position: //const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_; @@ -121,10 +122,11 @@ namespace ackermann_steering_controller integrate_fun_(linear * dt, angular * dt); } - void Odometry::setWheelParams(double wheel_separation_h, double wheel_radius) + void Odometry::setWheelParams(double wheel_separation_h, double front_wheel_radius, double rear_wheel_radius) { wheel_separation_h_ = wheel_separation_h; - wheel_radius_ = wheel_radius; + front_wheel_radius_ = front_wheel_radius; + rear_wheel_radius_ = rear_wheel_radius; } void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) From 0ba150ad7482541e1c43e0d94256a30649d62b25 Mon Sep 17 00:00:00 2001 From: Will Baker Date: Tue, 28 Jun 2022 09:51:49 -0700 Subject: [PATCH 2/4] Allow for default wheel_radius param to not break API - Cleanup formatting --- .../ackermann_steering_controller.h | 4 ++-- .../ackermann_steering_controller/odometry.h | 6 ++--- .../src/ackermann_steering_controller.cpp | 22 ++++++++++--------- .../src/odometry.cpp | 2 +- 4 files changed, 18 insertions(+), 16 deletions(-) diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.h b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.h index 796538f7a..82fefed9f 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.h +++ b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.h @@ -134,7 +134,7 @@ namespace ackermann_steering_controller{ /// Wheel radius (assuming it's the same for the left and right wheels): double front_wheel_radius_; - double rear_wheel_radius_; + double rear_wheel_radius_; /// Wheel separation and radius calibration multipliers: double wheel_separation_h_multiplier_; @@ -190,7 +190,7 @@ namespace ackermann_steering_controller{ const std::string rear_wheel_name, const std::string front_steer_name, bool lookup_wheel_separation_h, - bool lookup_front_wheel_radius, + bool lookup_front_wheel_radius, bool lookup_rear_wheel_radius); /** diff --git a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.h b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.h index 53d54a208..d4f64a6bd 100644 --- a/ackermann_steering_controller/include/ackermann_steering_controller/odometry.h +++ b/ackermann_steering_controller/include/ackermann_steering_controller/odometry.h @@ -143,8 +143,8 @@ namespace ackermann_steering_controller /** * \brief Sets the wheel parameters: radius and separation * \param wheel_separation Seperation between left and right wheels [m] - * \param front_wheel_radius Front (Steer) Wheel radius [m] - * \param rear_wheel_radius Rear (Drive) Wheel radius [m] + * \param front_wheel_radius Front (Steer) Wheel radius [m] + * \param rear_wheel_radius Rear (Drive) Wheel radius [m] */ void setWheelParams(double wheel_reparation_h, double front_wheel_radius, double rear_wheel_radius); @@ -194,7 +194,7 @@ namespace ackermann_steering_controller /// Wheel kinematic parameters [m]: double wheel_separation_h_; double front_wheel_radius_; - double rear_wheel_radius_; + double rear_wheel_radius_; /// Previous wheel position/state [rad]: double rear_wheel_old_pos_; diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 81eae955a..6cafa86c6 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -110,7 +110,7 @@ namespace ackermann_steering_controller{ , command_struct_() , wheel_separation_h_(0.0) , front_wheel_radius_(0.0) - , rear_wheel_radius_(0.0) + , rear_wheel_radius_(0.0) , wheel_separation_h_multiplier_(1.0) , wheel_radius_multiplier_(1.0) , steer_pos_multiplier_(1.0) @@ -218,14 +218,16 @@ namespace ackermann_steering_controller{ // If either parameter is not available, we need to look up the value in the URDF bool lookup_wheel_separation_h = !controller_nh.getParam("wheel_separation_h", wheel_separation_h_); - bool lookup_front_wheel_radius = !controller_nh.getParam("front_wheel_radius", front_wheel_radius_); - bool lookup_rear_wheel_radius = !controller_nh.getParam("rear_wheel_radius", rear_wheel_radius_); + double wheel_radius; + controller_nh.getParam("wheel_radius", wheel_radius); + bool lookup_front_wheel_radius = !controller_nh.param("front_wheel_radius", front_wheel_radius_, wheel_radius); + bool lookup_rear_wheel_radius = !controller_nh.param("rear_wheel_radius", rear_wheel_radius_, wheel_radius); if (!setOdomParamsFromUrdf(root_nh, rear_wheel_name, front_steer_name, lookup_wheel_separation_h, - lookup_front_wheel_radius, + lookup_front_wheel_radius, lookup_rear_wheel_radius)) { return false; @@ -235,11 +237,11 @@ namespace ackermann_steering_controller{ // to set the odometry parameters const double ws_h = wheel_separation_h_multiplier_ * wheel_separation_h_; const double fwr = wheel_radius_multiplier_ * front_wheel_radius_; - const double rwr = wheel_radius_multiplier_ * rear_wheel_radius_; + const double rwr = wheel_radius_multiplier_ * rear_wheel_radius_; odometry_.setWheelParams(ws_h, fwr, rwr); ROS_INFO_STREAM_NAMED(name_, "Odometry params : wheel separation height " << ws_h - << ", front wheel radius " << fwr + << ", front wheel radius " << fwr << ", rear wheel radius " << rwr); setOdomPubFields(root_nh, controller_nh); @@ -335,7 +337,8 @@ namespace ackermann_steering_controller{ last0_cmd_ = curr_cmd; // Set Command - const double wheel_vel = curr_cmd.lin/rear_wheel_radius_; // omega = linear_vel / radius, rear wheels are assumed to be drive wheels + // rear wheels are assumed to be drive wheels + const double wheel_vel = curr_cmd.lin/rear_wheel_radius_; // omega = linear_vel / radius rear_wheel_joint_.setCommand(wheel_vel); front_steer_joint_.setCommand(curr_cmd.ang); @@ -399,7 +402,7 @@ namespace ackermann_steering_controller{ const std::string rear_wheel_name, const std::string front_steer_name, bool lookup_wheel_separation_h, - bool lookup_front_wheel_radius, + bool lookup_front_wheel_radius, bool lookup_rear_wheel_radius) { if (!(lookup_wheel_separation_h || lookup_front_wheel_radius || lookup_rear_wheel_radius)) @@ -467,8 +470,7 @@ namespace ackermann_steering_controller{ } ROS_INFO_STREAM("Retrieved rear_wheel_radius: " << rear_wheel_radius_); } - - if (lookup_front_wheel_radius) + if (lookup_front_wheel_radius) { // Get front wheel radius if (!getWheelRadius(model->getLink(front_steer_joint->child_link_name), front_wheel_radius_)) diff --git a/ackermann_steering_controller/src/odometry.cpp b/ackermann_steering_controller/src/odometry.cpp index 3575a03b5..2e6be48d8 100644 --- a/ackermann_steering_controller/src/odometry.cpp +++ b/ackermann_steering_controller/src/odometry.cpp @@ -126,7 +126,7 @@ namespace ackermann_steering_controller { wheel_separation_h_ = wheel_separation_h; front_wheel_radius_ = front_wheel_radius; - rear_wheel_radius_ = rear_wheel_radius; + rear_wheel_radius_ = rear_wheel_radius; } void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size) From 26c8c9817b7328799a383878c64e0204f0787812 Mon Sep 17 00:00:00 2001 From: Will Baker Date: Tue, 28 Jun 2022 10:11:37 -0700 Subject: [PATCH 3/4] Dont lookup front/rear wheel radius if wheel_radius param is supplied --- .../src/ackermann_steering_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 6cafa86c6..dc26c5e26 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -219,7 +219,7 @@ namespace ackermann_steering_controller{ // If either parameter is not available, we need to look up the value in the URDF bool lookup_wheel_separation_h = !controller_nh.getParam("wheel_separation_h", wheel_separation_h_); double wheel_radius; - controller_nh.getParam("wheel_radius", wheel_radius); + bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius); bool lookup_front_wheel_radius = !controller_nh.param("front_wheel_radius", front_wheel_radius_, wheel_radius); bool lookup_rear_wheel_radius = !controller_nh.param("rear_wheel_radius", rear_wheel_radius_, wheel_radius); @@ -227,8 +227,8 @@ namespace ackermann_steering_controller{ rear_wheel_name, front_steer_name, lookup_wheel_separation_h, - lookup_front_wheel_radius, - lookup_rear_wheel_radius)) + lookup_front_wheel_radius && lookup_wheel_radius, + lookup_rear_wheel_radius && lookup_wheel_radius)) { return false; } From af6907b6100d05fe0118a17c0686039c8f561f25 Mon Sep 17 00:00:00 2001 From: Will Baker Date: Tue, 28 Jun 2022 10:55:37 -0700 Subject: [PATCH 4/4] Add mixed wheel radius tests --- ackermann_steering_controller/CMakeLists.txt | 1 + ...teering_controller_radius_mixed_param.test | 21 +++++++++++++++++++ 2 files changed, 22 insertions(+) create mode 100644 ackermann_steering_controller/test/ackermann_steering_controller_radius_mixed_param_test/ackermann_steering_controller_radius_mixed_param.test diff --git a/ackermann_steering_controller/CMakeLists.txt b/ackermann_steering_controller/CMakeLists.txt index a7725d409..3336eaf60 100644 --- a/ackermann_steering_controller/CMakeLists.txt +++ b/ackermann_steering_controller/CMakeLists.txt @@ -117,6 +117,7 @@ if (CATKIN_ENABLE_TESTING) add_rostest(test/ackermann_steering_controller_open_loop_test/ackermann_steering_controller_open_loop.test) add_rostest(test/ackermann_steering_controller_no_wheel_test/ackermann_steering_controller_no_wheel.test) add_rostest(test/ackermann_steering_controller_no_steer_test/ackermann_steering_controller_no_steer.test) + add_rostest(test/ackermann_steering_controller_radius_mixed_param_test/ackermann_steering_controller_radius_mixed_param.test) add_rostest(test/ackermann_steering_controller_radius_param_test/ackermann_steering_controller_radius_param.test) add_rostest(test/ackermann_steering_controller_radius_param_fail_test/ackermann_steering_controller_radius_param_fail.test) add_rostest(test/ackermann_steering_controller_separation_param_test/ackermann_steering_controller_separation_param.test) diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_radius_mixed_param_test/ackermann_steering_controller_radius_mixed_param.test b/ackermann_steering_controller/test/ackermann_steering_controller_radius_mixed_param_test/ackermann_steering_controller_radius_mixed_param.test new file mode 100644 index 000000000..872c37931 --- /dev/null +++ b/ackermann_steering_controller/test/ackermann_steering_controller_radius_mixed_param_test/ackermann_steering_controller_radius_mixed_param.test @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + +