Add a camera to erratic robot model

edit

Hi!
I'm running the demo_2dnav_slam .launch which simulates an erratic robot equipped with an hokuyo laser range finder in Gazebo.
I tried to move the robot using

rosrun erratic_teleop erratic_keyboard_teleop

and the simulation was fine.
Then I added a camera sensor to the robot model, adding this code to erratic_base.xacro

<joint name="my_camera_joint" type="fixed">

        <origin xyz="${top_size_x/2 -0.01 -0.04} 0 ${top_size_z/2 +0.025}" rpy="0 0 0" />
            <parent link="base_top_link" />
            <child link="my_camera_link" /> 

    </joint>

    <link name="my_camera_link">
            <inertial>
                <mass value="0.01"/>
                <origin xyz="0 0 0" />
                <inertia ixx="0.001" ixy="0.0" ixz="0.0"
                         iyy="0.01" iyz="0.0" izx="0.0"
                         izz="0.01" />
            </inertial>

            <visual>
               <origin xyz="0 0 0" rpy="0 0 0"/>
               <geometry>
                  <box size="0.05 0.05 0.05" />
               </geometry>
            </visual>

            <collision>
                <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001"/>
                </geometry>
            </collision>
        </link>

    <gazebo reference="my_camera_link">
     <sensor:camera name="my_camera_sensor">
     <imageFormat>R8G8B8</imageFormat>
         <hfov>90</hfov>
     <nearClip>0.01</nearClip>
     <farClip>100</farClip>
     <updateRate>20.0</updateRate>
        <controller:gazebo_ros_camera name="my_camera_controller" plugin="libgazebo_ros_camera.so">
          <alwaysOn>true</alwaysOn>
          <updateRate>20.0</updateRate>
      <imageTopicName>my_camera/image</imageTopicName>
          <frameName>my_camera_link</frameName>
          <interface:camera name="my_camera_iface" />
        </controller:gazebo_ros_camera>
      </sensor:camera>
      <turnGravityOff>true</turnGravityOff>
      <material>Erratic/White</material>
    </gazebo>

<joint name="my_camera_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
      <parent link="my_camera_link" />
      <child link="my_camera_optical_frame"/>
    </joint>
    <link name="my_camera_optical_frame">
      <inertial>
        <mass value="0.01" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.001"  ixy="0.0"  ixz="0.0"
                 iyy="0.001"  iyz="0.0"
                 izz="0.001" />
      </inertial>
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.001 0.001 0.001" />
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.001 0.001 0.001" />
        </geometry>
      </collision>
    </link>

To do this I followed the tutorial: link

Then I tried to see if the camera was working, with:

rosrun image_view image_view image:=/my_camera/image

And it worked.
The problem is that the new model of the erratic robot lost stability and when i tried to teleoperate the new model it rears up.
Do you know how to fix this problem?