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/include/ackermann_steering_controller/ackermann_steering_controller.h b/ackermann_steering_controller/include/ackermann_steering_controller/ackermann_steering_controller.h index 7894f2dbe..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 @@ -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..d4f64a6bd 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..dc26c5e26 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,17 @@ 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_); + double 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); if (!setOdomParamsFromUrdf(root_nh, rear_wheel_name, front_steer_name, lookup_wheel_separation_h, - lookup_wheel_radius)) + lookup_front_wheel_radius && lookup_wheel_radius, + lookup_rear_wheel_radius && lookup_wheel_radius)) { return false; } @@ -231,11 +236,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 +337,8 @@ namespace ackermann_steering_controller{ last0_cmd_ = curr_cmd; // Set Command - const double wheel_vel = curr_cmd.lin/wheel_radius_; // omega = linear_vel / radius + // 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); @@ -394,9 +402,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 +460,25 @@ 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..2e6be48d8 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) 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 @@ + + + + + + + + + + + + + + + + +