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
<?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
<?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:
```yaml
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:
```python
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:
```bash
source install/setup.bash
ros2 launch vox launch_sim.launch.py
```
Terminal 2:
```bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/diff_cont/cmd_vel_unstamped
```
Terminal 3:
```bash
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][1]][1]
[1]: https://i.sstatic.net/MBjfd.png
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.