Robot drifts away from goal and laserscan skews on random interval

I have to apologize for the vague and/or convoluted title. We don't know where the issue is ourselves. The robot I'm working on makes use of ROS Kinetic, the standard navigation stack with `amcl`, `teb_local_planner`, `laser_scan_matcher` and `move_base`. It is a front-wheeled differential drive with two caster wheels on the back with a rectangular body/footprint, built on the chassis of an electric wheelchair. The issue we're facing is happening on a random interval: Whenever we set a goal in rviz, the robot does move towards his goal. After a random amount of time or after a certain distance has been traveled, two errors could occur: The robot loses track of his position by "teleporting" on the map within rviz, commonly setting himself approx. 1 meter to the right. The laser scan output could also become skewed, throwing off its odometry. Each of these erratic behaviours is visible within rviz and the console output can mention the errors `Map Update/Control loop missed its desired rate <...>` As a result, the robot would keep on its last set velocity (`cmd_vel` output) and rviz would no longer be able to set and display a new goal, rendering the robot unresponsive. We're able to clear `cmd_vel` by using manual control (`teleop_twist_joy`) and stopping the robot that way. Upon hitting the `reset` button on the bottom-right in rviz, we progressively lose the local and global costmap per press, accompanied by the warning "No map provided". Rebooting the ROS environment will restore everything until the "drift" occurs again. We can consistently recreate this behaviour by having the robot drive various distances, believing it appears more often when traversing small distances of approximately 2-3 meters, on longer distances i.e 5-20 meters the errors occure less often, but the laserscan becoming askew is still prevalent. We're hoping to get video footage of the erroneous behaviour in order to demonstrate what's happening both physically and in rviz. Below are the common configuration files requested: - navigation.launch - base_local_planner_params.yaml controller_frequency: 2.0 recovery_behavior_enabled: false clearing_rotation_allowed: false recovery_behaviors: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}] TebLocalPlannerROS: odom_topic: odom map_frame: /odom # Trajectory teb_autosize: True dt_ref: 0.3 dt_hysteresis: 0.1 global_plan_overwrite_orientation: True max_global_plan_lookahead_dist: 2.0 feasibility_check_no_poses: 4 # Robot max_vel_x: 0.7 min_vel_x: 0.3 max_vel_x_backwards: 0.5 max_vel_theta: 0.7 acc_lim_x: 0.7 acc_lim_theta: 0.7 min_turning_radius: 0.4 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" type: "line" line_start: [0.0, 0.0] line_end: [-0.6, 0.0] radius: 0.2 # GoalTolerance xy_goal_tolerance: 0.2 yaw_goal_tolerance: 0.1 free_goal_vel: False # Obstacles min_obstacle_dist: 0.3 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 1.0 obstacle_poses_affected: 30 costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" costmap_converter_spin_thread: True costmap_converter_rate: 5 # Optimization no_inner_iterations: 4 no_outer_iterations: 3 optimization_activate: True optimization_verbose: False penalty_epsilon: 0.1 weight_max_vel_x: 2 weight_max_vel_theta: 1 weight_acc_lim_x: 0.5 weight_acc_lim_theta: 6.0 weight_kinematics_nh: 1000 weight_kinematics_forward_drive: 1 weight_kinematics_turning_radius: 1 weight_optimaltime: 1 weight_obstacle: 50 # Homotopy Class Planner enable_homotopy_class_planning: True enable_multithreading: True simple_exploration: True max_number_classes: 2 roadmap_graph_no_samples: 15 roadmap_graph_area_width: 5 h_signature_prescaler: 0.5 h_signature_threshold: 0.1 obstacle_keypoint_offset: 0.1 obstacle_heading_threshold: 0.45 visualize_hc_graph: False - costmap_common_params.yaml plugins: - {name: static_layer, type: 'costmap_2d::StaticLayer'} - {name: obstacle_layer, type: 'costmap_2d::ObstacleLayer'} - {name: inflation_layer, type: 'costmap_2d::InflationLayer'} - {name: sonar, type: "range_sensor_layer::RangeSensorLayer"} footprint: [[0.25,-0.31],[0.25,0.31],[-0.69,0.31],[-0.69,-0.31]] footprint_padding: 0.03 inflation_layer: inflation_radius: 0.4 cost_scaling_factor: 2.58 sonar: topics: ['/sonar_data'] obstacle_layer: combination_method: 1 enabled: true footprint_clearing_enabled: true max_obstacle_height: 0.6 observation_sources: scan obstacle_range: 2.0 raytrace_range: 2.0 scan: {clearing: true, data_type: LaserScan, expected_update_rate: 0, marking: true, topic: /scan_filtered} - global_costmap_params.yaml global_costmap: global_frame: map robot_base_frame: /base_link update_frequency: 5.0 publish_frequency: 1.0 static_map: true rolling_window: false resolution: 0.05 transform_tolerance: 1.0 map_type: costmap - local_costmap_params.yaml local_costmap: global_frame: odom robot_base_frame: base_link update_frequency: 5.0 publish_frequency: 1.0 static_map: true rolling_window: true width: 5.0 height: 5.0 resolution: 0.05 transform_tolerance: 1.0 map_type: costmap inflation_layer: inflation_radius: 0.2 cost_scaling_factor: 10