Poor localization and goal tracking by navigation stack

edit

I am trying to simulate the navigation stack in gazebo & rviz. I have developed a differential drive mobile robot and configured the navigation stack for the model. I have built a map with willow garage world and launched my self-navigation launch file.

The robot shows poor localization results in rviz and fails to reach the goal. It navigates till 50% of goal's path then looses the control and getting aborted. Where would be the problem and how to fix the issue.

I am also getting the following warnings

[ WARN] [1454605148.779801697, 21.830000000]: Failed to transform initial pose in time (Lookup would require extrapolation into the future. Requested time 21.825000000 but the latest data is at time 21.821000000, when looking up transform from frame [base_link] to frame [map])

[ WARN] [1454605512.623590430, 69.784000000]: Map update loop missed its desired rate of 2.0000Hz... the loop actually took 0.6400 seconds

PS: I am using non gpu laser because i got some problem with my gpu_ray laser. Below is my launch and costmap files

<launch>
  <!-- Load the TortoiseBot URDF model into the parameter server -->
  <param name="robot_description" textfile="$(find jmbot2_description)/urdf/jmbot1.2.urdf" />
  <!-- Start Gazebo with a world containing a large building-->
  <include file="$(find gazebo_ros)/launch/willowgarage_world.launch"/>
  <!-- include file="$(find gazebo_ros)/launch/empty_world.launch"/ -->
  <!-- Spawn a TortoiseBot in Gazebo, taking the description from the
       parameter server -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" 
    args="-param robot_description -urdf -model jmbot1.2 -x 8 -y -8" />

<!-- Convert /joint_states messages published by Gazebo to /tf messages,
       e.g., for rviz-->
  <node name="robot_state_publisher" pkg="robot_state_publisher"
        type="robot_state_publisher"/>
  <node name="map_server" pkg="map_server" type="map_server"
        args="$(find mapping2)/src/maps/mapnew.yaml"  />

<include file="$(find amcl)/examples/amcl_diff.launch"/>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find jmbot2_description)/src/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find jmbot2_description)/src/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find jmbot2_description)/src/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find jmbot2_description)/src/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find jmbot2_description)/src/base_local_planner_params.yaml" command="load" />
    <param name="controller_frequency" value="5.0"/>
  </node>

</launch>

base_local_planner_params.yaml

TrajectoryPlannerROS:
  holonomic_robot: false
  meter_scoring: true

costmap_common_params.yaml

obstacle_range: 2.5
raytrace_range: 3.0
robot_radius: 0.5
inflation_radius: 0.55
observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: hokuyo_link, data_type: LaserScan, topic: scan, marking: true, clearing: true}

global_costmap_params.yaml

global_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 2.0
  static_map: true

local_costmap_params.yaml

local_costmap:
  global_frame: /odom
  robot_base_frame: base_link
  update_frequency: 2.0
  publish_frequency: 1.0
  static_map: false
  rolling_window: true
  width: 10.0
  height: 10.0
  resolution: 0.050000