Problem with ROS Navigation on a real robot

edit

Hello,

I have been building a robot for a university project and working with ROS Navigation stack.

I have got the robot to work in Gazebo simulation using Gazebo plugins and managed to send goal poses through Rviz successfully. However now I am trying to reproduce the same on the real robot but the goal planning isn’t going so well. It seems that the localization and the odometry update isn’t working properly.

When I give the robot very close goal poses it moves with no problems, but when I give a goal which is slightly more far on the map (a couple of feet) it starts to go off track or in case of a 90 degree turn, the robot keeps going straight and then it takes a big turn or a detour in order to return to it’s goal and gives these warnings:

[ WARN] [1488103550.240345429]: Costmap2DROS transform timeout. Current time: 1488103550.2400, global_pose stamp: 1488103548.1647, tolerance: 2.0000
[ WARN] [1488103550.241602412]: Could not get robot pose, cancelling pose reconfiguration

Below is an image explaining a scenario with the desired path the robot should take and the path it takes with the ROS planner:

Here is my setup:

I am using real Kinect + fake laser depthimage_to_laserscan in combination with AMCL and Move Base.

I am publishing the odometry using this class since I am using roboclaw motor shield: https://github.com/sonyccd/roboclaw_r...

I have based my move_base on this example and only changed the names of topics: https://github.com/turtlebot/turtlebo...

Below is my TF graph:

My navigation launch file:

<arg name="scan_topic" default="kinect_scan"/> 
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_link"/>
  <arg name="global_frame_id" default="map"/>

<!-- DG: Kinect cloud to laser scan -->
<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
    <remap from="image"     to="/camera/depth/image_raw"/>
    <remap from="camera_info" to="/camera/depth/camera_info"/>
    <remap from="scan" to="$(arg scan_topic)"/>
    <param name="range_max" type="double" value="4"/>
</node>

<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0 0 0 0 0 0 /base_link /camera_depth_frame 100"/>

  <!-- Map server -->
  <arg name="map_file" default="$(find rovytwo_navigation)/maps/empty/blank_map.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <arg name="use_map_topic"   default="false"/>
  <arg name="initial_pose_x"  default="0.0"/>
  <arg name="initial_pose_y"  default="0.0"/>
  <arg name="initial_pose_a"  default="0.0"/>
  <node pkg="amcl" type="amcl" name="amcl" output="screen">
    <param name="use_map_topic"             value="$(arg use_map_topic)"/>

    <param name="odom_model_type"           value="diff"/>
    <param name="odom_alpha5"               value="0.1"/>
    <param name="gui_publish_rate"          value="10.0"/> <!-- -->
    <param name="laser_max_beams"             value="60"/>
    <param name="laser_max_range"           value="12.0"/>
    <param name="min_particles"             value="500"/>
    <param name="max_particles"             value="2000"/>
    <param name="kld_err"                   value="0.05"/>
    <param name="kld_z"                     value="0.99"/>
    <param name="odom_alpha1"               value="0.2"/>
    <param name="odom_alpha2"               value="0.2"/>

    <param name="odom_alpha3"               value="0.2"/>
    <param name="odom_alpha4"               value="0.2"/>
    <param name="laser_z_hit"               value="0.5"/>
    <param name="laser_z_short"             value="0.05"/>
    <param name="laser_z_max"               value="0.05"/>
    <param name="laser_z_rand ...