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;