Hi all,

I know that this is a multi-times repeated question but unfortunately I didn't find a solution to my problem. I tried both teblocalplanner and neolocalplanner for my differential drive, back-wheeled robot, but when it reaches the goal, even though it may outputs "Goal Reached", movebase doesn't recognize the goal as being reached successfully, and moves on to the recovery behaviors before aborting the mission. I tried with less xytolerance but in my opinion it should work with 0.1m tolerance. I get the feeling that the reason may be that I send the goals in the base_footprint frame instead of map, but it would make it complicated to send it in the map frame. Any suggestion-opinion is welcome. A similar problem identified here: https://answers.ros.org/question/240877/under-what-circumstances-will-actionlib-not-mark-a-movebasegoal-as-successful/

PS: As a side-problem and since I already uploaded my files, I would be glad if anyone identifies why my neolocalplanner periodically accelerates and decelerates instead of doing a smooth move. I believe it has something to do with the frequencies.

costmapcommonparams:

map_type: costmap

footprint: [[-0.2, 0.4], [0.8, 0.4], [0.8, -0.4], [-0.2, -0.4]]
# robot_radius: 0.4 #  footprint of the robot or the radius of the robot if it is circular
inflation_radius: 0.4 # the robot will treat all paths that stay 0.55 meters or more away from obstacles as having equal obstacle cost
transform_tolerance: 1.0
# always_send_full_costmap: true
# subscribe_to_updates: true


obstacle_layer:
 enabled: true
 obstacle_range: 2.5 # the robot will only update its map with information about obstacles that are within 2.5 meters of the base
 raytrace_range: 3.0 # the robot will attempt to clear out space in front of it up to 3.0 meters
 inflation_radius: 0.1
 track_unknown_space: true
 combination_method: 1

 observation_sources: laser_scan_sensor
 laser_scan_sensor: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true} #topic: scan_filtered


inflation_layer:
  enabled: true
  observation_persistence: 0.0
  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.6  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true
  observation_persistence: 0.0
  map_topic:            "map"

global_costmap

global_costmap:
  global_frame: spraybase/map #map
  robot_base_frame: spraybase/base_footprint
  update_frequency: 30 # Set this as high as the cpu resources allow
  publish_frequency: 30 #2.5
  # static_map: false
  static_map: true
  rolling_window: false
  track_unknown_space: true
  width: 1024
  height: 1024
  origin_x: -51.25
  origin_y: -51.25
  resolution: 0.05

  plugins:
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

local_costmap

local_costmap:
  global_frame: spraybase/odom # odom_combined
  robot_base_frame: spraybase/base_footprint
  update_frequency: 5. # 5.0
  publish_frequency: 5. # 5.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  resolution: 0.03
  # transform_tolerance: 1.0

  plugins:
  #  - {name: static_layer,        type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer,      type: "costmap_2d::VoxelLayer"}
   - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

teblocalplanner:

TebLocalPlannerROS:

 odom_topic: odom

 # Trajectory

 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1 #default: 0.01
 max_samples: 50 #default: 3 
 global_plan_overwrite_orientation: True
 allow_init_with_backwards_motion: True
 max_global_plan_lookahead_dist: 0.5
 global_plan_viapoint_sep: -1 #default: 0.01
 global_plan_prune_distance: 1 #default: 0
 exact_arc_length: False #default: True
 feasibility_check_no_poses: 4
 publish_feedback: False #default: True

 # Robot

 max_vel_x: 0.3
 max_vel_x_backwards: 0.2
 max_vel_y: 0.1
 max_vel_theta: 0.15 #default: 1.0 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
 acc_lim_x: 0.15
 acc_lim_y: 0.15
 acc_lim_theta: 0.15

 # ********************** Holonomic robot parameters ********************
 min_turning_radius: 0.0 #(for holonomic) - default: 0.4 Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
 wheelbase: 0.58               # Wheelbase of our robot
 #cmd_angle_instead_rotvel: True # stage simulator takes the angle instead of the rotvel as input (twist message)
 # ********************************************************************

 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "line"
   radius: 0.4 # for type "circular"
   line_start: [0.0, 0.0] # for type "line"
   line_end: [0.58, 0.0] # for type "line"
   front_offset: 0.0 # for type "two_circles"
   front_radius: 0.0 # for type "two_circles"
   rear_offset: 0.0 # for type "two_circles"
   rear_radius: 0.0 # for type "two_circles"
   vertices: [ [-0.077,-0.1], [0.3,-0.1], [0.3,0.1], [-0.077,0.1] ] # for type "polygon"
   #vertices: [ [0.156,0.192], [-0.156,0.192], [-0.156,-0.276], [0.156,-0.276] ]


 # GoalTolerance

 xy_goal_tolerance: 0.4
 yaw_goal_tolerance: 0.1
 free_goal_vel: False
 complete_global_plan: True

 # Obstacles

 min_obstacle_dist: 0.4 # This value must also include our robot's expansion, since footprint_model is set to "line". (not anymore, it set to "Polygon" 27.07 - Leon)
 inflation_dist: 0.0
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.5
 obstacle_poses_affected: 0

 dynamic_obstacle_inflation_dist: 0.0
 include_dynamic_obstacles: True 

 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization

 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 obstacle_cost_exponent: 0
 weight_max_vel_x: 2
 weight_max_vel_y: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_y: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1 # default: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1 # must be > 0
 weight_shortest_path: 0
 weight_obstacle: 100
 weight_inflation: 0.2
 weight_dynamic_obstacle: 10 # not in use yet
 weight_dynamic_obstacle_inflation: 0.2
 weight_viapoint: 1
 weight_adapt_factor: 2

 # Homotopy Class Planner

 enable_homotopy_class_planning: False
 enable_multithreading: True
 max_number_classes: 2
 selection_cost_hysteresis: 1.0
 selection_prefer_initial_plan: 0.95
 selection_obst_cost_scale: 1.0
 selection_alternative_time_cost: False

 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 roadmap_graph_area_length_scale: 1.0
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_heading_threshold: 0.45
 switching_blocking_period: 0.0
 viapoints_all_candidates: True
 delete_detours_backwards: True
 max_ratio_detours_duration_best_duration: 3.0
 visualize_hc_graph: False
 visualize_with_time_as_z_axis_scale: False

# Recovery

 shrink_horizon_backup: True
 shrink_horizon_min_duration: 10
 oscillation_recovery: True
 oscillation_v_eps: 0.1
 oscillation_omega_eps: 0.1
 oscillation_recovery_min_duration: 10
 oscillation_filter_duration: 10

neolocalplanner:

NeoLocalPlanner:

  # The x acceleration limit of the robot in meters/sec^2
  acc_lim_x : 0.15 #default: 0.25

  # The y acceleration limit of the robot in meters/sec^2
  acc_lim_y : 0.20

  # The rotational acceleration limit of the robot in radians/sec^2
  acc_lim_theta : 0.15 #default: 0.8

  # The maximum x velocity for the robot in m/s.
  max_vel_x : 0.3 # default: 0.8

  # The minimum x velocity for the robot in m/s, negative for backwards motion.
  min_vel_x : -0.2

  # The maximum y velocity for the robot in m/s
  max_vel_y : 0.3 # default: 0.5

  # The minimum y velocity for the robot in m/s
  min_vel_y : -0.3

  # The absolute value of the maximum rotational velocity for the robot in rad/s
  max_rot_vel : 0.15 # default: 0.8

  # The absolute value of the minimum rotational velocity for the robot in rad/s
  min_rot_vel : 0.1 # default: 0.1

  # The absolute value of the maximum translational velocity for the robot in m/s
  max_trans_vel : 0.3 # default: 0.8

  # The absolute value of the minimum translational velocity for the robot in m/s
  min_trans_vel : 0.1

  # The tolerance in radians for the controller in yaw/rotation when achieving its goal
  yaw_goal_tolerance : 0.05 #default: 0.01

  # The tolerance in meters for the controller in the x & y distance when achieving a goal
  xy_goal_tolerance : 0.2 #default: 0.02

  # How long to fine tune for goal position after reaching tolerance limits [s]
  goal_tune_time : 3

  # How far to predict control pose into the future based on latest odometry [s]
  lookahead_time : 0.4

  # How far to look ahead when computing path orientation [m]
  lookahead_dist : 0.5

  # Threshold yaw error below which we consider to start moving [rad]
  start_yaw_error : 0.5

  # Gain when adjusting final x position for goal [1/s]
  pos_x_gain : 1

   # Gain when adjusting y position (holonomic only) [1/s]
  pos_y_gain : 1

  # Gain for adjusting yaw when not translating, or in case of holonomic drive  [1/s]
  static_yaw_gain : 3

  # Gain for x cost avoidance (holonomic only)
  cost_x_gain : 0.2

  # Gain for y cost avoidance (holonomic only)
  cost_y_gain : 0.2

  # How far ahead to compute y cost gradient (constant offset) [m]
  cost_y_lookahead_dist : 0

  # How far ahead to compute y cost gradient (dynamic offset) [s]
  cost_y_lookahead_time : 2

  # Gain for yaw cost avoidance
  cost_yaw_gain : 2

  # Gain for final control low pass filter
  low_pass_gain : 0.2

  # Max cost to allow, above we slow down to min_trans_vel or even stop
  max_cost : 0.95

  # Max velocity based on curvature [rad/s]
  max_curve_vel : 0.3

  # Max distance to goal when looking for it [m]
  max_goal_dist : 0.5

  # Max distance allowable for backing up (zero = unlimited) [m]
  max_backup_dist : 0

  # Minimal distance for stopping [m]
  min_stop_dist : 0.3

  # If robot has differential drive, holonomic otherwise
  differential_drive : true

  # If to constrain final control step to keep last commanded direction
  constrain_final: true

  pdist_scale: 1.2

  path_distance_bias: 0.15

  # global_frame_id: spraybase/odom