[ROS 2] Fix how wheel properties are retrieved in Ackermann Plugin

gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp

@@ -39,6 +39,22 @@ class GazeboRosAckermannDrivePrivate;
 
       <update_rate>100.0</update_rate>
 
+      <!-- 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 -->
       <front_left_joint>front_left_wheel_joint</front_left_joint>
       <front_right_joint>front_right_wheel_joint</front_right_joint>

gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp

@@ -321,35 +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());
 
-  // Update wheel radiu 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));
-
-  // 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();
+  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;