1
$\begingroup$

I'm trying to make a 4 wheel robot with an Ackermann steering. I've seen there is a plugin in ros2 for this but I don't know how to use it properly.

I've created the four link for each wheel and the base_link but when I write the command in the terminal with the topic /cmd_vel I can only move the steering wheels, not the whole robot.

This is what I put in the terminal:

ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "linear: x: 2.0 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: 0.0"

But it only turn the wheels in Z directions as if the robot was fixed to the ground. Has anyone worked with this plugin? Any idea?

I have this structure:

  • 1x base_link
  • 4x wheel
  • 2x art_wheel (for the front wheels)

My code here:

<?xml version="1.0"?>

<robot name="robot_exteriores">


    <link name="base_link">
        <visual>
            <origin
                xyz="0 0 0"
                rpy="1.57 0 0" />
            <geometry>
                <mesh
                    filename="package://robot_desc/meshes/base_link.stl" scale="0.001 0.001 0.001" />
            </geometry>
        </visual>
        <collision>
            <origin
                xyz="0 0 0"
                rpy="1.57 0 0" />
            <geometry>
                <mesh
                    filename="package://robot_desc/meshes/base_link.stl" scale="0.001 0.001 0.001" /> 
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 4.668e-3 7.502e-3"/>
            <mass value= "4"/>
            <inertia ixx="1.294e-1" ixy="0" ixz="0" iyy="1.869e-1" iyz="-4e-6" izz="1.4672e-1"/>  
        </inertial>
    </link>

    
    <link name="wheel_1">
        <visual>
            <origin
            xyz="0 0 0"
            rpy="1.57 0 -1.57" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/rueda.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </visual>
        <collision>
            <origin
            xyz="0 0 0"
            rpy="1.57 0 -1.57" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/rueda.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz= "-0.232 0.2274 0.1"/>
            <mass value= "1"/>
            <inertia ixx="5.936e-3" ixy="0" ixz="0" iyy="3.744e-3" iyz="0" izz="3.744e-3"/>
        </inertial>
    </link>
   

    <link name="art_w_1">
        <visual>
            <origin
            xyz="0 0 0"
            rpy="0 0 0" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/art_w.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </visual>
        <collision>
            <origin
            xyz="0 0 0"
            rpy="0 0 0" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/art_w.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz= "-0.1834 0.2274 0.1"/>
            <mass value= "0.048"/>
            <inertia ixx="6,88e-6" ixy="4,679e-6" ixz="0" iyy="6,445e-6" iyz="0" izz="1,289e-5"/>
        </inertial>
    </link>


    <link name="wheel_2">
        <visual>
            <origin
            xyz="0 0 0"
            rpy="1.57 0 1.57" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/rueda.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </visual>
        <collision>
            <origin
            xyz="0 0 0"
            rpy="1.57 0 1.57" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/rueda.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz= "-0.232 0.2274 0.1"/>
            <mass value= "1"/>
            <inertia ixx="5.936e-3" ixy="0" ixz="0" iyy="3.744e-3" iyz="0" izz="3.744e-3"/>
        </inertial>
    </link>


    <link name="art_w_2">
        <visual>
            <origin
            xyz="0 0 0"
            rpy="0 3.1416 0" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/art_w.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </visual>
        <collision>
            <origin
            xyz="0 0 0"
            rpy="0 3.1416 0" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/art_w.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz= "0.1834 0.2274 0.1"/>
            <mass value= "0.048"/>
            <inertia ixx="6,88e-6" ixy="4,679e-6" ixz="0" iyy="6,445e-6" iyz="0" izz="1,2885e-5"/>
        </inertial>
    </link>


    <link name="wheel_3">
        <visual>
            <origin
            xyz="0 0 0"
            rpy="1.57 0 -1.57" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/rueda.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </visual>
        <collision>
            <origin
            xyz="0 0 0"
            rpy="1.57 0 -1.57" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/rueda.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz= "-0.232 0.2274 0.1"/>
            <mass value= "1"/>
            <inertia ixx="5.936e-3" ixy="0" ixz="0" iyy="3.744e-3" iyz="0" izz="3.744e-3"/>
        </inertial>
    </link>


    <link name="wheel_4">
        <visual>
            <origin
            xyz="0 0 0"
            rpy="1.57 0 1.57" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/rueda.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </visual>
        <collision>
            <origin
            xyz="0 0 0"
            rpy="1.57 0 1.57" />
            <geometry>
                <mesh 
                    filename="package://robot_desc/meshes/rueda.stl" scale="0.001 0.001 0.001"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz= "-0.232 0.2274 0.1"/>
            <mass value= "1"/>
            <inertia ixx="5.936e-3" ixy="0" ixz="0" iyy="3.744e-3" iyz="0" izz="3.744e-3"/>
        </inertial>
    </link>

   

<!-- //////////////////////////////// ACKERMANN STEERING JOINTS ////////////////////////////////////// -->   

    
    <joint name="base_art_w_1" type="revolute">
        <origin xyz="-0.160 0.2525 0"/>
        <parent link="base_link"/>
        <child link="art_w_1"/>
        <limit upper="0.5236" lower="-0.5236" velocity="20" effort="10"/>
        <axis xyz= "0 0 1" />
        <dynamics damping="0.05" friction="0.2"/>
    </joint>

    <joint name="art_wheel_1" type="continuous">
        <origin xyz="-0.080 0 0"/>
        <parent link="art_w_1"/>
        <child link="wheel_1"/>
        <axis xyz= "-1 0 0" />
        <dynamics damping="0.05" friction="0.01"/>
    </joint>
    
    <joint name="base_art_w_2" type="revolute">
        <origin xyz="0.160 0.2525 0"/>
        <parent link="base_link"/>
        <child link="art_w_2"/>
        <limit upper="0.5236" lower="-0.5236" velocity="20" effort="10"/>
        <axis xyz= "0 0 1" />
        <dynamics damping="0.05" friction="0.2"/>
    </joint>

    <joint name="art_wheel_2" type="continuous">
        <origin xyz="0.080 0 0"/>
        <parent link="art_w_2"/>
        <child link="wheel_2"/>
        <axis xyz= "1 0 0" />
        <dynamics damping="0.05" friction="0.01"/>
    </joint>
 
    
<!-- ////////////////////////////////////// JOINTS ///////////////////////////////////////////////// --> 
    
    <joint name="base_wheel_3" type="continuous">
        <origin xyz="-0.2265 -0.2525 0"/>
        <parent link="base_link"/>
        <child link="wheel_3"/>
        <axis xyz= "-1 0 0" />
        <dynamics damping="0.05" friction="0.01"/>
    </joint>

    <joint name="base_wheel_4" type="continuous">
        <origin xyz="0.2265 -0.2525 0"/>
        <parent link="base_link"/>
        <child link="wheel_4"/>
        <axis xyz= "1 0 0" />
        <dynamics damping="0.05" friction="0.01"/>
    </joint>

    
<!-- //////////////////////////////// GAZEBO PROPERTIES //////////////////////////////////////////// -->
<!--<gazebo reference="base_link">
    <mu1>100</mu1>  
    <mu2>100</mu2>
    <kp>1000000</kp>
    <kd>1</kd>
</gazebo>-->

<gazebo reference="wheel_1">
        <mu1>1</mu1>
        <mu2>1</mu2>
        <kp>1000000</kp>
        <kd>0.1</kd>
</gazebo>

<gazebo reference="wheel_2">
        <mu1>1</mu1>
        <mu2>1</mu2>
        <kp>1000000</kp>
        <kd>0.1</kd>
</gazebo>

<gazebo reference="wheel_3">
        <mu1>1</mu1>
        <mu2>1</mu2>
        <kp>1000000</kp>
        <kd>0.1</kd>
</gazebo>

<gazebo reference="wheel_4">
        <mu1>1</mu1>
        <mu2>1</mu2>
        <kp>1000000</kp>
        <kd>0.1</kd>
</gazebo>

        
<!-- ////////////////////////////////////// PLUGIN ////////////////////////////////////////////////////-->
<gazebo>
<plugin name="gazebo_ros_ackermann_drive" filename="libgazebo_ros_ackermann_drive.so">


  <update_rate>100.0</update_rate>

  <!-- wheels -->
  <front_left_joint>art_wheel_1</front_left_joint>
  <front_right_joint>art_wheel_2</front_right_joint>
  <rear_left_joint>base_wheel_3</rear_left_joint>
  <rear_right_joint>base_wheel_4</rear_right_joint>
  <left_steering_joint>base_art_w_1</left_steering_joint>
  <right_steering_joint>base_art_w_2</right_steering_joint>
  

  <!-- Max absolute steer angle for tyre in radians-->
  <!-- Any cmd_vel angular z greater than this would be capped -->
  <max_steer>0.5236</max_steer>

  <!-- Max absolute steering angle of steering wheel -->
  <max_steering_angle>7.85</max_steering_angle>

  <!-- Max absolute linear speed in m/s -->
  <max_speed>50</max_speed>

  <!-- PID tuning -->
  <left_steering_pid_gain>1500 0 1</left_steering_pid_gain>
  <left_steering_i_range>0 0</left_steering_i_range>
  <right_steering_pid_gain>1500 0 1</right_steering_pid_gain>
  <right_steering_i_range>0 0</right_steering_i_range>
  <linear_velocity_pid_gain>1000 0 1</linear_velocity_pid_gain>
  <linear_velocity_i_range>0 0</linear_velocity_i_range>

  <!--input puesto por mí-->
  <!--<command_topic>cmd_vel</command_topic>-->

  <!-- output -->
  <publish_odom>true</publish_odom>
  <publish_odom_tf>true</publish_odom_tf>
  <publish_wheel_tf>true</publish_wheel_tf>
  <publish_distance>true</publish_distance>

  <odometry_frame>odom_demo</odometry_frame>
  <robot_base_frame>base_link</robot_base_frame>

</plugin>
</gazebo> 
</robot>

Thank you very much.

I'm working with ROS2 Foxy and Gazebo 11.

Best regards,

Dani.

$\endgroup$

1 Answer 1

1
$\begingroup$

I nearly met the same question as you, but I fixed it eventually. With the source code of the Ackermann drive plugin, you can see that the plugin is using the collision geometry radius of the "REAR_RIGHT" wheel as its parameter and something else.

impl_->wheel_radius_ = impl_->CollisionRadius(
impl_->joints_[GazeboRosAckermannDrivePrivate::REAR_RIGHT]->GetChild()->GetCollision(id));

See Code

double GazeboRosAckermannDrivePrivate::CollisionRadius(const gazebo::physics::CollisionPtr & _coll)
{
  if (!_coll || !(_coll->GetShape())) {
    return 0;
  }
  if (_coll->GetShape()->HasType(gazebo::physics::Base::CYLINDER_SHAPE)) {
    gazebo::physics::CylinderShape * cyl =
      dynamic_cast<gazebo::physics::CylinderShape *>(_coll->GetShape().get());
    return cyl->GetRadius();
  } else if (_coll->GetShape()->HasType(gazebo::physics::Base::SPHERE_SHAPE)) {
    gazebo::physics::SphereShape * sph =
      dynamic_cast<gazebo::physics::SphereShape *>(_coll->GetShape().get());
    return sph->GetRadius();
  }
  return 0;
}

See Code

So,if you're using a .stl or other meshes as the collision geometry of your rear right wheel, the plugin will fail and display without any erro info. Besides, the "continuous" joints are need to add <limit> tags to run the plugin, otherwise the tf of the wheels will fail.

You can also see the Issue as a reference.

$\endgroup$
1
  • $\begingroup$ Hi Glens! Finally I updated to Humble but a I think that it doesn't matter the ros2 distro. Anyway I've just solved the problem and I used the gazebo_plugin_ackermann.sdf as a reference where they had a Toyota Prius which you can use to prove the plugin. In my case I think the issue was the art_wheel link instead of a simple virtual link. Because on the Toyota Prius the use a "universal joint" which has 2 degrees of freedom. This kind of joint is only used in the sdf file of gazebo. There I also saw the collision geometry that you comment and I changed to sphere shape and it worked. $\endgroup$ Jan 16 at 19:45

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.