Allow different sized wheels for ackermann controller

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)

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

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_;

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,25 +218,31 @@ 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;
     }
 
     // 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;

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)

ackermann_steering_controller/test/ackermann_steering_controller_radius_mixed_param_test/ackermann_steering_controller_radius_mixed_param.test

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Load common test stuff -->
+  <include file="$(find ackermann_steering_controller)/test/common/launch/ackermann_steering_common.launch" />
+
+  <!-- Override robot_description with sphere wheel urdf -->
+  <param name="robot_description"
+      command="$(find xacro)/xacro '$(find ackermann_steering_controller)/test/common/urdf/ackermann_steering_bot_sphere_wheels.xacro'" />
+  <!-- Provide the radius, since the bot's wheels are spheres, not cylinders -->
+  <param name="ackermann_steering_bot_controller/front_wheel_radius" value="0.10"/>
+  <param name="ackermann_steering_bot_controller/rear_wheel_radius" value="0.12"/>
+
+  <!-- Controller test -->
+  <test test-name="ackermann_steering_controller_wheel_radius_param_test"
+        pkg="ackermann_steering_controller"
+        type="ackermann_steering_controller_test"
+        time-limit="80.0">
+    <remap from="cmd_vel" to="ackermann_steering_bot_controller/cmd_vel" />
+    <remap from="odom" to="ackermann_steering_bot_controller/odom" />
+  </test>
+</launch>