gazebo-controller for car-like robot

edit

Hello everyone,

I can't seem to find a working controller for my car-like robot. This is the model I am using:

<?xml version="1.0"?>
<robot name="$(arg roboname)" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- Included URDF/XACRO Files -->
  <xacro:include filename="$(find chassis_description)/urdf/materials.urdf.xacro" />
  <xacro:include filename="$(find chassis_description)/urdf/chassis_wheels.urdf.xacro" />

  <!-- PROPERTY LIST -->
  <!--All units in m-kg-s-radians unit system -->
  <property name="M_PI" value="3.1415926535897931" />

  <!-- Main Body-base -->
  <property name="base_x_size" value="4.68" /> 
  <property name="base_y_size" value="1.27" /> 
  <property name="base_z_size" value="0.2" />
  <property name="base_mass" value="800" /> <!-- in kg-->

  <!--Inertial macros for the box and cylinder. Units are kg*m^2-->
  <macro name="box_inertia" params="m x y z">
    <inertia  ixx="${m*(y*y+z*z)/12}" ixy = "0" ixz = "0"
              iyy="${m*(x*x+z*z)/12}" iyz = "0"
              izz="${m*(x*x+z*z)/12}" /> 
  </macro>

  <!-- BASE-FOOTPRINT -->
  <!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin -->
  <link name="base_footprint">
    <inertial>
      <mass value="0.0001" />
      <origin xyz="0 0 0" />
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
          iyy="0.0001" iyz="0.0" 
          izz="0.0001" />
    </inertial>
    <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <box size="0.001 0.001 0.001" />
        </geometry>
    </visual>
  </link>

  <gazebo reference="base_footprint">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>

  <joint name="base_footprint_joint" type="fixed">
  <origin xyz="0 0 ${wheel_radius - base_z_origin_to_wheel_origin}" rpy="0 0 0" />
    <parent link="base_footprint"/>
    <child link="base_link" />
  </joint>

  <!-- BASE-LINK -->
  <!--Actual body/chassis of the robot-->
  <link name="base_link">
    <inertial>
      <mass value="${base_mass}" />
      <origin xyz="0 0 0" />
      <!--The 3x3 rotational inertia matrix. -->
      <box_inertia  m="${base_mass}" x="${base_x_size}" y="${base_y_size}" z="${base_z_size}"/> 
    </inertial>    
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <box size="${base_x_size} ${base_y_size} ${base_z_size}"/>
      </geometry>
      <material name="Grey" />
    </visual>  
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0 " />
      <geometry>
        <box size="${base_x_size} ${base_y_size} ${base_z_size}"/>
      </geometry>
    </collision>     
  </link>
  <gazebo reference="base_link">
    <material>Gazebo/Grey</material>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>

  <!-- WHEELs -->
  <wheel fb="front" lr="right" parent="front_right_wheel_steering_block" translateX="1" translateY="-1" flipY="-1" base_x_origin_to_wheel_origin="1.66"/>
  <wheel fb="front" lr="left" parent="front_left_wheel_steering_block" translateX="1" translateY="1" flipY="-1" base_x_origin_to_wheel_origin="1.66"/>
  <wheel fb="back" lr="right" parent="base_link" translateX="-1" translateY="-1" flipY="-1" base_x_origin_to_wheel_origin="1.14"/>
  <wheel fb="back" lr="left" parent="base_link" translateX="-1" translateY="1" flipY="-1" base_x_origin_to_wheel_origin="1.14"/>

    <joint name="front_right_wheel_joint" type="continuous">
      <parent link="front_right_wheel_steering_block"/>
      <child link="front_right_wheel"/>
      <origin xyz="0 0 0.125" rpy="1.570796 0 0" /> 
      <axis xyz="0 1 0" rpy="0 0 0" />
      <limit effort="100" velocity="100"/>
      <joint_properties damping="0.0" friction="0.0"/>
    </joint>

    <joint name="front_left_wheel_joint" type="continuous">
      <parent link="front_left_wheel_steering_block"/>
      <child link="front_left_wheel"/>
      <origin xyz="0 0 0.125" rpy="-1.570796 0 0" /> 
      <axis xyz="0 1 0" rpy="0 0 0" />
      <limit effort="100" velocity="100"/>
      <joint_properties damping="0.0" friction="0.0"/>
    </joint>

    <joint name="back_left_wheel_joint" type="continuous">
      <parent link="base_link"/>
      <child link="back_left_wheel"/>
      <origin ...