Robot doesn't take a turn to face the goal in Gazebo.

edit

I have created a custom robot model in Gazebo, with a Skid Steer drive controller plugin. I tried this video, but my robot doesn't take any turn, and it's moving to a straight line for ever. This is the code from the video :

#! /usr/bin/env python

import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Point, Twist
from math import atan2

x = 0.0
y = 0.0
theta = 0.0

def newOdom (msg):
    global x
    global y
    global theta

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y

    rot_q = msg.pose.pose.orientation
    (roll, pitch, theta) = euler_from_quaternion ([rot_q.x, rot_q.y, rot_q.z, rot_q.w])

rospy.init_node ("speed_controller")

sub = rospy.Subscriber("/odom", Odometry, newOdom)
pub = rospy.Publisher("/cmd_vel",Twist, queue_size=1)

speed = Twist()

r = rospy.Rate(4)

goal = Point ()
goal.x = 100
goal.y = 100

while not rospy.is_shutdown():
      inc_x = goal.x - x
      inc_y = goal.y - y

      angle_to_goal = atan2 (inc_y, inc_x)

      if abs(angle_to_goal - theta) > 0.1:
          speed.linear.x = 0.0
          speed.angular.z = 1.0
      else:
          speed.linear.x = 0.5
          speed.angular.z = 0.0

      pub.publish(speed)
      r.sleep()

This is the URDF parts for the wheels:

  <joint name="front_axle_to_front_left_wheel" type="continuous">
    <parent link="front_axle"/>
    <child link="front_left_wheel"/>
    <origin rpy="0 0 0" xyz="1.0000000e-04 -1.0000000e-04 -1.78000000e-00"/>
    <axis xyz="0 0 -1.0000000e+00"/>
    <limit effort="-1.0" lower="-1.79769e+308" upper="1.79769e+308" velocity="-1.0"/>
  </joint>
  <link name="front_left_wheel">
    <inertial>
      <mass value="0.1"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
    <collision name="collision">
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.87" radius="1.275"/>
      </geometry>
    </collision>
    <visual name="visual">
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.87" radius="1.275"/>
      </geometry>
    </visual>
  </link>

  <joint name="front_axle_to_front_right_wheel" type="continuous">
    <parent link="front_axle"/>
    <child link="front_right_wheel"/>
    <origin rpy="0 0 0" xyz="1.0000000e-04 1.0000000e-04 1.78000000e-00"/>
    <axis xyz="0 0 1.0000000e+00"/>
    <limit effort="-1.0" lower="-1.79769e+308" upper="1.79769e+308" velocity="-1.0"/>
  </joint>
  <link name="front_right_wheel">
    <inertial>
      <mass value="0.1"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
    <collision name="collision">
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.87" radius="1.275"/>
      </geometry>
    </collision>
    <visual name="visual">
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.87" radius="1.275"/>
      </geometry>
    </visual>
  </link>

  <joint name="back_axle_to_back_left_wheel" type="continuous">
    <parent link="back_axle"/>
    <child link="back_left_wheel"/>
    <origin rpy="0 0 0" xyz="-1.0000000e-04 -1.0000000e-04 -1.78000000e-00"/>
    <axis xyz="0 0 -1.0000000e+00"/>
    <limit effort="-1.0" lower="-1.79769e+308" upper="1.79769e+308" velocity="-1.0"/>
  </joint>
  <link name="back_left_wheel">
    <inertial>
      <mass value ...