Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Open
wants to merge 4 commits into
base: ros2
Choose a base branch
from
Open

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
11402ae
Retrieve wheel parameters from sdf
Yadunund Sep 14, 2022
402fe31
update documentation
Yadunund Sep 14, 2022
45a1857
Cleanup
Yadunund Sep 14, 2022
7410afe
Default to inferring wheel properties from mesh
Yadunund Jun 11, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 16 additions & 0 deletions gazebo_plugins/include/gazebo_plugins/gazebo_ros_ackermann_drive.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Copy link

@cschindlbeck cschindlbeck Aug 13, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This should be

Suggested change
<wheel_radius>0.3</front_left_joint>
<wheel_radius>0.3</wheel_radius>

<!-- 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>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Could we please include a comment to indicate that this means "wheel_seperation" in ackermann language? Should help people to understand what really is just from reading.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done 7410afe


<!-- wheels -->
<front_left_joint>front_left_wheel_joint</front_left_joint>
<front_right_joint>front_right_wheel_joint</front_right_joint>
Expand Down
81 changes: 52 additions & 29 deletions gazebo_plugins/src/gazebo_ros_ackermann_drive.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down