Default to inferring wheel properties from mesh

Signed-off-by: Yadunund <yadunund@openrobotics.org>

gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp

@@ -39,9 +39,20 @@ class GazeboRosAckermannDrivePrivate;
 
       <update_rate>100.0</update_rate>
 
-      <!-- kinematics -->
+      <!-- Ackermann geometry and kinematic parameters-->
+
+      <!-- By default this plugin will attempt to infer the wheel radius,
+      wheelbase and track width from the geometry and poses of the links defined
+      as front and rear wheels. This only works when the collision meshes are
+      cylinder or sphere primitives. If the collision mesh for the model
+      needs to be more complex, you must set this param to false and specific
+      the values of the wheel params below. -->
+      <wheel_params_from_collision_mesh>true</wheel_params_from_collision_mesh>
+      <!-- The radius of the wheel in meters. Used when wheel_params_from_collision_mesh is false. -->
       <wheel_radius>0.3</front_left_joint>
+      <!-- The distance between the front and rear sets of wheels in meters. Used when wheel_params_from_collision_mesh is false -->
       <wheel_base>2.0</wheel_base>
+      <!-- The separate distance between the two steered wheels in meters. Used when wheel_params_from_collision_mesh is false -->
       <track_width>1.2</track_width>
 
       <!-- wheels -->

gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp

@@ -321,10 +321,58 @@ void GazeboRosAckermannDrive::Load(gazebo::physics::ModelPtr _model, sdf::Elemen
     "linear_velocity_i_range", ignition::math::Vector2d::Zero).first;
   impl_->pid_linear_vel_.Init(pid.X(), pid.Y(), pid.Z(), i_range.Y(), i_range.X());
 
-  // Get the kinematic parameters
-  impl_->wheel_radius_ = _sdf->Get<double>("wheel_radius", 0.3).first;
-  impl_->wheel_separation_ = _sdf->Get<double>("track_width", 1.2).first;
-  impl_->wheel_base_ = _sdf->Get<double>("wheel_base", 2.0).first;
+  const bool wheel_params_from_mesh = _sdf->Get<bool>(
+    "wheel_params_from_collision_mesh", true).first;
+  if (wheel_params_from_mesh)
+  {
+    // Update wheel radius for wheel from SDF collision objects
+    // assumes that wheel link is child of joint (and not parent of joint)
+    // assumes that wheel link has only one collision
+    // assumes all wheel of both rear wheels of same radii
+    unsigned int id = 0;
+    impl_->wheel_radius_ = impl_->CollisionRadius(
+      impl_->joints_[GazeboRosAckermannDrivePrivate::REAR_RIGHT]->GetChild()->GetCollision(id));
+
+    if (std::abs(impl_->wheel_radius_ - 0.0) < 1e-3)
+    {
+      RCLCPP_ERROR(
+        impl_->ros_node_->get_logger(),
+        "Unable to infer radius of the wheels from the collision mesh of the rear right wheel. "
+        "The only supported collision mesh geometries are cylinder and sphere. To continue using "
+        "your collision mesh, set wheel_params_from_collision_mesh plugin param to false and "
+        "provide values for params wheel_radius, track_width and wheel_base."
+      );
+      return;
+    }
+    // Compute wheel_base, front wheel separation, and rear wheel separation
+    // first compute the positions of the 4 wheel centers
+    // again assumes wheel link is child of joint and has only one collision
+    auto front_right_center_pos = impl_->joints_[GazeboRosAckermannDrivePrivate::FRONT_RIGHT]->
+      GetChild()->GetCollision(id)->WorldPose().Pos();
+    auto front_left_center_pos = impl_->joints_[GazeboRosAckermannDrivePrivate::FRONT_LEFT]->
+      GetChild()->GetCollision(id)->WorldPose().Pos();
+    auto rear_right_center_pos = impl_->joints_[GazeboRosAckermannDrivePrivate::REAR_RIGHT]->
+      GetChild()->GetCollision(id)->WorldPose().Pos();
+    auto rear_left_center_pos = impl_->joints_[GazeboRosAckermannDrivePrivate::REAR_LEFT]->
+      GetChild()->GetCollision(id)->WorldPose().Pos();
+
+    auto distance = front_left_center_pos - front_right_center_pos;
+    impl_->wheel_separation_ = distance.Length();
+
+    // to compute wheelbase, first position of axle centers are computed
+    auto front_axle_pos = (front_left_center_pos + front_right_center_pos) / 2;
+    auto rear_axle_pos = (rear_left_center_pos + rear_right_center_pos) / 2;
+    // then the wheelbase is the distance between the axle centers
+    distance = front_axle_pos - rear_axle_pos;
+    impl_->wheel_base_ = distance.Length();
+  }
+  else
+  {
+    // Get the wheel parameters from sdf params.
+    impl_->wheel_radius_ = _sdf->Get<double>("wheel_radius", 0.3).first;
+    impl_->wheel_separation_ = _sdf->Get<double>("track_width", 1.2).first;
+    impl_->wheel_base_ = _sdf->Get<double>("wheel_base", 2.0).first;
+  }
 
   // Update rate
   auto update_rate = _sdf->Get<double>("update_rate", 100.0).first;