p3dx wrong navigation

edit

Hello, I have a problem with navigation p3dx. As You can see on a video below. My model moving in random direction. P3dx has a problem with a little teleportation as see on video.

p3dx error video

I have Ubuntu 16.04 and ROS Kinetic

My config files:

amcl.launch

<?xml version="1.0"?>

<launch>
  <arg name="map_file" default="$(find p3dx_control)/map2.yaml"/>
  <arg name="use_map_topic" default="true"/>
  <arg name="scan_topic" default="p3dx/laser/scan" />


  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />


  <node pkg="amcl" type="amcl" name="amcl">
    <remap from="scan" to="mybot/laser/scan"/>
    <param name="odom_frame_id" value="odom"/>
    <param name="odom_model_type" value="diff-corrected"/>
    <param name="base_frame_id" value="base_link"/>
    <param name="update_min_d" value="0.5"/>
    <param name="update_min_a" value="1.0"/>
  </node>

</launch>

tf.launch

<?xml version="1.0"?>

<launch>
<node name="joint_state_publisher" pkg="joint_state_publisher"
type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

</launch>

move_base.launch

<?xml version="1.0"?>

<launch>

  <arg name="no_static_map" default="false"/>
  <arg name="base_global_planner" default="navfn/NavfnROS"/>
<!-- <arg name="base_local_planner" default="dwa_local_planner/DWAPlannerROS"/> -->
  <arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/>


<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

    <param name="base_global_planner" value="$(arg base_global_planner)"/>
    <param name="base_local_planner" value="$(arg base_local_planner)"/> 
    <rosparam file="$(find p3dx_control)/config/planner.yaml" command="load"/>

    <rosparam file="$(find p3dx_control)/config/costmap_common.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find p3dx_control)/config/costmap_common.yaml" command="load" ns="local_costmap" />


    <rosparam file="$(find p3dx_control)/config/costmap_local.yaml" command="load" ns="local_costmap" />
    <param name="local_costmap/width" value="10.0"/>
    <param name="local_costmap/height" value="10.0"/>


    <rosparam file="$(find p3dx_control)/config/global_costmap.yaml" command="load" ns="global_costmap" unless="$(arg no_static_map)"/>

    <rosparam file="$(find p3dx_control)/config/global_costmap_static.yaml" command="load" ns="global_costmap" if="$(arg no_static_map)"/>
    <param name="global_costmap/width" value="100.0" if="$(arg no_static_map)"/>
    <param name="global_costmap/height" value="100.0" if="$(arg no_static_map)"/>

</node>
</launch>

planner.yaml

controller_frequency: 2.0
recovery_behaviour_enabled: true

NavfnROS:
  allow_unknown: true # Specifies whether or not to allow navfn to create plans that traverse unknown space.
  default_tolerance: 0.1 # A tolerance on the goal point for the planner.

TrajectoryPlannerROS:
  # Robot Configuration Parameters
  acc_lim_x: 2.5
  acc_lim_theta:  3.2

  max_vel_x: 1.0
  min_vel_x: 0.0

  max_vel_theta: 1.0
  min_vel_theta: -1.0
  min_in_place_vel_theta: 0.2

  holonomic_robot: false
  escape_vel: -0.1

  # Goal Tolerance Parameters
  yaw_goal_tolerance: 0.1
  xy_goal_tolerance: 0.1
  latch_xy_goal_tolerance: false

  # Forward Simulation Parameters
  sim_time: 2.0
  sim_granularity: 0.02
  angular_sim_granularity: 0.02
  vx_samples: 6
  vtheta_samples: 20
  controller_frequency: 2.0

  # Trajectory scoring parameters
  meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).
  occdist_scale:  0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01
  pdist_scale: 0.75 ...