0
$\begingroup$

I've been building a 4 wheel differential drive robot (all 4 wheels have motors) and I've noticed that when I control the robot, in Gazebo it barely turns while in RViz it turns a lot more. This causes a lot of desync and I've been trying to solve it for quite some time now.

I'm working on a VM, VMWare Workstation 16 running Ubuntu 22.04.3 LTS. I have ROS2 Iron and I'm using ros2_control for moving the robot. For now, I'm using RViz2 and Gazebo to simulate the robot (I don't know which version of Gazebo I have, it auto-installed based on my ROS2 version).

You can find all the code here https://github.com/ClauPan/Vox/tree/main/src

Here is my urdf for the robot structure (the numbers for the different parameters are based on the dimensions of my real robot, which I plan to use this code for):

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:include filename="colors.xacro"/>

    <!-- BASE LINK -->

    <link name="base_link"></link>

    <!-- BASE FOOTPRINT LINK -->
    <joint name="base_footprint_joint" type="fixed">
        <parent link="base_link"/>
        <child link="base_footprint"/>
        <origin xyz="0 0 0" rpy="0 0 0"/>
    </joint>

    <link name="base_footprint"></link>

    <!-- CHASSIS LINK -->

    <joint name="chassis_joint" type="fixed">
        <parent link="base_link"/>
        <child link="chassis"/>
        <origin xyz="-0.105 0 0"/>
    </joint>

    <link name="chassis">
        <parent link="chassis_joint"/>
        <visual>
            <geometry>
                <box size="0.21 0.135 0.0475"/>
            </geometry>
            <material name="white"/>
        </visual>
        <collision>
            <geometry>
                <box size="0.21 0.135 0.0475"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="0.67"/>
            <inertia 
                ixx="${(1/12) * 0.67 * (0.135*0.135+0.0475*0.0475)}" ixy="0.0" ixz="0.0"
            iyy="${(1/12) * 0.67 * (0.21*0.21+0.0475*0.0475)}" iyz="0.0"
                izz="${(1/12) * 0.67 * (0.21*0.21+0.135*0.135)}"
            />
        </inertial>
    </link>

    <gazebo reference="chassis">
        <material>Gazebo/White</material>
    </gazebo>

    <joint name="front_left_wheel_joint" type="continuous">
        <parent link="chassis"/>
        <child link="front_left_wheel"/>
        <origin xyz="-0.08 0.1145 0" rpy="-${pi/2} 0 0"/>
        <axis xyz="0 0 1"/>
    </joint>

    <link name="front_left_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.06" length="0.06"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <!-- <cylinder radius="0.06" length="0.06"/> -->
                <sphere radius="0.06"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="0.17"/>
            <inertia 
                ixx="${(1/12) * 0.17 * (3*0.06*0.06 + 0.06*0.06)}" ixy="0.0" ixz="0.0"
            iyy="${(1/12) * 0.17 * (3*0.06*0.06 + 0.06*0.06)}" iyz="0.0"
                izz="${(1/12) * 0.17 * (0.06*0.06)}"
            />
        </inertial>
    </link>

    <gazebo reference="front_left_wheel">
        <material>Gazebo/Blue</material>
    </gazebo>

    <joint name="front_right_wheel_joint" type="continuous">
        <parent link="chassis"/>
        <child link="front_right_wheel"/>
        <origin xyz="-0.08 -0.1145 0" rpy="${pi/2} 0 0"/>
        <axis xyz="0 0 -1"/>
    </joint>

    <link name="front_right_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.06" length="0.06"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <!-- <cylinder radius="0.06" length="0.06"/> -->
                <sphere radius="0.06"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="0.17"/>
            <inertia 
                ixx="${(1/12) * 0.17 * (3*0.06*0.06 + 0.06*0.06)}" ixy="0.0" ixz="0.0"
            iyy="${(1/12) * 0.17 * (3*0.06*0.06 + 0.06*0.06)}" iyz="0.0"
                izz="${(1/12) * 0.17 * (0.06*0.06)}"
            />
        </inertial>
    </link>

    <gazebo reference="front_right_wheel">
        <material>Gazebo/Blue</material>
    </gazebo>

    <joint name="back_left_wheel_joint" type="continuous">
        <parent link="chassis"/>
        <child link="back_left_wheel"/>
        <origin xyz="0.08 0.1145 0" rpy="-${pi/2} 0 0"/>
        <axis xyz="0 0 1"/>
    </joint>

    <link name="back_left_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.06" length="0.06"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <!-- <cylinder radius="0.06" length="0.06"/> -->
                <sphere radius="0.06"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="0.17"/>
            <inertia 
                ixx="${(1/12) * 0.17 * (3*0.06*0.06 + 0.06*0.06)}" ixy="0.0" ixz="0.0"
            iyy="${(1/12) * 0.17 * (3*0.06*0.06 + 0.06*0.06)}" iyz="0.0"
                izz="${(1/12) * 0.17 * (0.06*0.06)}"
            />
        </inertial>
    </link>

    <gazebo reference="back_left_wheel">
        <material>Gazebo/Blue</material>
    </gazebo>

    <joint name="back_right_wheel_joint" type="continuous">
        <parent link="chassis"/>
        <child link="back_right_wheel"/>
        <origin xyz="0.08 -0.1145 0" rpy="${pi/2} 0 0"/>
        <axis xyz="0 0 -1"/>
    </joint>

    <link name="back_right_wheel">
        <visual>
            <geometry>
                <cylinder radius="0.06" length="0.06"/>
            </geometry>
            <material name="blue"/>
        </visual>
        <collision>
            <geometry>
                <!-- <cylinder radius="0.06" length="0.06"/> -->
                <sphere radius="0.06"/>
            </geometry>
        </collision>
        <inertial>
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <mass value="0.17"/>
            <inertia 
                ixx="${(1/12) * 0.17 * (3*0.06*0.06 + 0.06*0.06)}" ixy="0.0" ixz="0.0"
            iyy="${(1/12) * 0.17 * (3*0.06*0.06 + 0.06*0.06)}" iyz="0.0"
                izz="${(1/12) * 0.17 * (0.06*0.06)}"
            />
        </inertial>
    </link>

    <gazebo reference="back_right_wheel">
        <material>Gazebo/Blue</material>
    </gazebo>

</robot>

This is the urdf for my ros2_control:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

    <ros2_control name="GazeboSystem" type="system">
        <hardware>
            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
        </hardware>
        <joint name="front_left_wheel_joint">
            <command_interface name="velocity">
                <param name="min">-10</param>
                <param name="max">10</param>
            </command_interface>
            <state_interface name="velocity"></state_interface>
            <state_interface name="position"></state_interface>
        </joint>
        <joint name="back_left_wheel_joint">
            <command_interface name="velocity">
                <param name="min">-10</param>
                <param name="max">10</param>
            </command_interface>
            <state_interface name="velocity"></state_interface>
            <state_interface name="position"></state_interface>
        </joint>
        <joint name="front_right_wheel_joint">
            <command_interface name="velocity">
                <param name="min">-10</param>
                <param name="max">10</param>
            </command_interface>
            <state_interface name="velocity"></state_interface>
            <state_interface name="position"></state_interface>
        </joint>    
        <joint name="back_right_wheel_joint">
            <command_interface name="velocity">
                <param name="min">-10</param>
                <param name="max">10</param>
            </command_interface>
            <state_interface name="velocity"></state_interface>
            <state_interface name="position"></state_interface>
        </joint>
    </ros2_control>

    <gazebo> 
        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
            <parameters>$(find vox)/config/my_controllers.yaml</parameters>
        </plugin>
    </gazebo>
</robot>

This is my .yaml file for the controller:

controller_manager:
  ros__parameters:
    update_rate: 30
    use_sim_time: true

    diff_cont:
      type: diff_drive_controller/DiffDriveController

    joint_broad:
      type: joint_state_broadcaster/JointStateBroadcaster

diff_cont:
  ros__parameters:

    publish_rate: 50.0

    base_frame_id: base_link

    left_wheel_names: ['front_left_wheel_joint', 'back_left_wheel_joint']
    right_wheel_names: ['front_right_wheel_joint', 'back_right_wheel_joint']
    wheel_separation: 0.229
    wheel_radius: 0.06

    use_stamped_vel: false

    open_loop: true

    # wheels_per_side: 2
    # wheel_separation_multiplier: 0.08
    # left_wheel_radius_multiplier: x
    # right_wheel_radius_multiplier: x

    # odom_frame_id: odom
    pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
    # open_loop: x
    # enable_odom_tf: x

    # cmd_vel_timeout: x
    # publis_limited_velocity: x
    # velocity_roling_window_size: x

    # linear.x.has_velocity_limits: true
    # linear.x.has_acceleration_limits: true
    # linear.x.has_jerk_limits: false
    # linear.x.max_velocity: 1
    # linear.x.min_velocity: NAN
    # linear.x.max_acceleration: 3
    # linear.x.min_acceleration: NAN
    # linear.x.max_jerk: NAN
    # linear.x.min_jerk: NAN

    # angular.z.has_velocity_limits: true
    # angular.z.has_acceleration_limits: true
    # angular.z.has_jerk_limits: false
    # angular.z.max_velocity: 1
    # angular.z.min_velocity: NAN
    # angular.z.max_acceleration: 3
    # angular.z.min_acceleration: NAN
    # angular.z.max_jerk: NAN
    # angular.z.min_jerk: NAN

# joint_broad:
#   ros_parameters:

And this is my launch file:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node

def generate_launch_description():
    package_name = "vox"
    rsp = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory(package_name), "launch", "rsp.launch.py")
        ]),
        launch_arguments={"use_sim_time": "true", "use_ros2_control": "true"}.items()
    )

    gazebo_params_file = os.path.join(get_package_share_directory(package_name), "config", "gazebo_params.yaml")
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(get_package_share_directory("gazebo_ros"), "launch", "gazebo.launch.py")
        ]),
        launch_arguments={'extra_gazebo_args': '--ros-args --param-files' + gazebo_params_file}.items()
    )
    spawn_entity = Node(
        package="gazebo_ros", executable="spawn_entity.py",
        arguments=["-topic", "robot_description", "-entity", "vox"],
        output="screen"
    )

    diff_drive_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["diff_cont"],
    )
    
    joint_broad_spawner = Node(
        package="controller_manager",
        executable="spawner",
        arguments=["joint_broad"],
    )

    return LaunchDescription([
        rsp, 
        gazebo, 
        spawn_entity,
        diff_drive_spawner,
        joint_broad_spawner
    ])

The way I start everything up is by running the following commands: Terminal 1:

source install/setup.bash
ros2 launch vox launch_sim.launch.py

Terminal 2:

ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/diff_cont/cmd_vel_unstamped

Terminal 3:

rviz2

After pressing "l" once, which is "turn right", the RViz2 representation turned a lot more then the Gazebo one. desync between rviz2 and gazebo

I haven't noticed this very much until I started implementing SLAM into the project. This difference in turn messes up mapping a lot.

One other thing I've noticed is that if I go into selection mode in RViz and select a wheel, 1, 2 or all values in the "orientation" field of all wheels keeps changing by itself, even when there is no input. I don't know if this is related, just an observation

I would appreciate any and all suggestions to fix this drift between RViz2 and Gazebo. If you need anything else from my side, I'll post them ASAP.

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

You use the diff_drive_controller in open-loop mode. This means that odometry gets calculated from the commands only, there is no sensor feedback.

My first guess is that the wheels slip on the floor and the robot is not moving as commanded due to the physics properties. Can you verify this by sending a twist command with certain values and observe the rotation, maybe just visually or you add a gazebo plugin for publishing the ground truth

Improve this answer
$\endgroup$
2
  • $\begingroup$ Heyo. I've tried commenting out open-loop and making it false. Same result. Also, I've tried using this command ros2 topic pub --once /diff_cont/cmd_vel_unstamped geometry_msgs/msg/Twist "{linear: {x: 2.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.8}}" to trigger the movement manually. It has the same effect, it moved a lot more in RViz then in Gazebo. I've installed the plugin but I'm not sure how to use it. Sorry, I'm fairly new to ros $\endgroup$
    – Lokplart
    Commented Feb 22 at 9:27
  • $\begingroup$ if wheel slipping is the problem, then open_loop: false neither won't help you. I haven't used the plugin, you have to find out yourself how to use it. The easiest way: Use a twist command with something like angular.z=pi/2 and measure the time until it made a full turn. $\endgroup$
    – Christoph Froehlich
    Commented Feb 22 at 10:26

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.