diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp
index afc1ef676..1e460f9cc 100644
--- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp
+++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp
@@ -39,9 +39,20 @@ class GazeboRosAckermannDrivePrivate;
100.0
-
+
+
+
+ true
+
0.3
+
2.0
+
1.2
diff --git a/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp b/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp
index aa57e3322..c83ab404e 100644
--- a/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp
+++ b/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("wheel_radius", 0.3).first;
- impl_->wheel_separation_ = _sdf->Get("track_width", 1.2).first;
- impl_->wheel_base_ = _sdf->Get("wheel_base", 2.0).first;
+ const bool wheel_params_from_mesh = _sdf->Get(
+ "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("wheel_radius", 0.3).first;
+ impl_->wheel_separation_ = _sdf->Get("track_width", 1.2).first;
+ impl_->wheel_base_ = _sdf->Get("wheel_base", 2.0).first;
+ }
// Update rate
auto update_rate = _sdf->Get("update_rate", 100.0).first;