From 11402aee8a2526706b6ce5d47229b35d19c32eae Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 14 Sep 2022 18:03:22 +0800 Subject: [PATCH 1/4] Retrieve wheel parameters from sdf Signed-off-by: Yadunund --- .../gazebo_ros_ackermann_drive.hpp | 5 +++ .../src/gazebo_ros_ackermann_drive.cpp | 35 ++++--------------- 2 files changed, 11 insertions(+), 29 deletions(-) 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 91d6ce3ac..44882c1d8 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp @@ -40,6 +40,11 @@ class GazeboRosAckermannDrivePrivate; 100.0 + 0.3 + 2.0 + 1.2 + + front_left_wheel_joint front_right_wheel_joint rear_left_wheel_joint diff --git a/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp b/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp index 8f536598f..b5edd27cb 100644 --- a/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp @@ -321,35 +321,12 @@ 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(); + // Get the wheel radius + impl_->wheel_radius_ = _sdf->Get("wheel_radius", 0.3).first; + + // Get the wheel separation + 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; From 402fe312a55e516b9d3fd98c955e321d5596e549 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 14 Sep 2022 18:06:57 +0800 Subject: [PATCH 2/4] update documentation Signed-off-by: Yadunund --- .../include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 44882c1d8..afc1ef676 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp @@ -39,12 +39,12 @@ class GazeboRosAckermannDrivePrivate; 100.0 - + 0.3 2.0 1.2 - + front_left_wheel_joint front_right_wheel_joint rear_left_wheel_joint From 45a18571cb72d9bc1cbb51472562e618950bdfd9 Mon Sep 17 00:00:00 2001 From: Yadunund Date: Wed, 14 Sep 2022 18:09:55 +0800 Subject: [PATCH 3/4] Cleanup Signed-off-by: Yadunund --- gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp b/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp index b5edd27cb..aa57e3322 100644 --- a/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp +++ b/gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp @@ -321,10 +321,8 @@ 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 wheel radius + // Get the kinematic parameters impl_->wheel_radius_ = _sdf->Get("wheel_radius", 0.3).first; - - // Get the wheel separation impl_->wheel_separation_ = _sdf->Get("track_width", 1.2).first; impl_->wheel_base_ = _sdf->Get("wheel_base", 2.0).first; From 7410afe37274be91661275230afbfe1d6dd630cb Mon Sep 17 00:00:00 2001 From: Yadunund Date: Sun, 11 Jun 2023 23:37:41 +0800 Subject: [PATCH 4/4] Default to inferring wheel properties from mesh Signed-off-by: Yadunund --- .../gazebo_ros_ackermann_drive.hpp | 13 ++++- .../src/gazebo_ros_ackermann_drive.cpp | 56 +++++++++++++++++-- 2 files changed, 64 insertions(+), 5 deletions(-) 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;