Local planner behaves unordinary

edit

Hello,

I am using base_local_planner (TrajectoryPlannerROS with DWA enabled) and it works in general, it sends the commands to cmd_vel and my robot interprets them quite good using this simple inverse kinematics formula (I am cancelling negative velocities as my robot cannot handle them well):

v_right = (cmd_angular_z*self.ROBOT_BASE_LENGTH)/2.0 + cmd_linear_x
v_left = cmd_linear_x*2.0 - v_right

if v_right < 0:
    v_right = 0
    v_left *= 2
elif v_left < 0:
    v_left = 0
    v_right *= 2

But when I set a goal, the local planner publishes short and strange plans. Even though it does like that, sometimes my robot is able to get to the goal and stop but sometimes it collides to the objects.

Here is the video: LINK

The red line is the local planner and green line is the global planner.

Here is my TrajectroyPlannerROS params:

recovery_behavior_enabled: true clearing_rotation_allowed: true

TrajectoryPlannerROS:
  acc_lim_x: 1
  acc_lim_y: 0 # as we have a differential robot
  acc_lim_theta: 1.6
  max_vel_x: 0.6 # configure, was 0.2
  min_vel_x: 0.2 # was 0.4
  max_vel_y: 0.0  # zero for a differential drive robot
  min_vel_y: 0.0
  max_vel_theta: 1.6 # 0.35
  min_vel_theta: 3.0 #-0.35
  min_in_place_vel_theta: 3.0 #0.25
  min_rot_vel: 3.0
  escape_vel: 0.0

  holonomic_robot: false

  meter_scoring: true

  xy_goal_tolerance: 0.3 # 0.15, 0.3->about 20 cm
  yaw_goal_tolerance: 0.20 # 0.25, about 11 degrees
  latch_xy_goal_tolerance: true  #false

  sim_time: 3 # <2 values not enough time for path planning and can't go through narrow
  sim_granularity: 0.05 # How frquent should the points on trajectory be examined
  angular_sim_granularity: 0.025
  vx_samples: 20
  vy_samples: 0 # 0 for differential robots
  vtheta_samples: 5 # 20
  controller_frequency: 15

  pdist_scale: 0.8
  gdist_scale: 0.4
  occdist_scale: 0.01
  oscillation_reset_dist: 0.2
  heading_scoring: true
  heading_lookahead: 1 # how far to look ahead in meters
  heading_scoring_timestep: 0.8
  dwa: true
  publish_cost_grid_pc: false
  global_frame_id: odom
  simple_attractor: false

  reset_distance: 4

  prune_plan: true