Robot footprint fluctuates when 2d pose estimate is applied on RVIz.

edit

Hi, So I am new to ROS and I am facing a major headache for the past week. Specifically, I am having a problem that is actually shown here. Whenever I set the 2d pose estimate of Robot, the footprint and costmap kind of fluctuates around the map. I am using an amcl node with a static map, so whenever I give a pose estimate the particle cloud changes correctly. Due to this problem it can't estimate an accurate path to the goal node and the path fluctuates as well. My launch file codes are below. Please get me rid of this problem. Thanks!

Trajectory Planner

TrajectoryPlannerROS:
  max_vel_x: 0.4
  min_vel_x: 0.2
  max_rotational_vel: 0.5
  max_vel_theta: 1.5
  min_vel_theta: -1.5
  min_in_place_rotational_vel: 0.25
  min_in_place_vel_theta: 0.7
  escape_vel: -0.25
  acc_lim_theta: 0.3
  acc_lim_x: 0.5
  acc_lim_Y: 0.07
  holonomic_robot: false
  meter_scoring: true
  xy_goal_tolerance: 0.15
  yaw_goal_tolerance: 0.3
  latch_xy_goal_tolerance: true

Local costmap params

local_costmap:
  global_frame: odom
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 10.0
  static_map: false
  rolling_window: true
  width: 2
  height: 2
  resolution: 0.025

Global costmap params

global_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 2.0
  static_map: true
  publish_frequency: 1.0
  width: 15
  height: 15
  resolution: 0.1

Costmap common params

obstacle_range: 5.0
raytrace_range: 6.0

max_obstacle_height: 0.2
min_obstacle_height: 0.05

robot_radius: 0.20
inflation_radius: 0.15

transform_tolerance: 1.0
map_type: costmap
cost_scaling_factor: 100

map_topic: /map
subscribe_to_updates: true

observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan , marking: true, clearing: true}

My Robot Configuration File

<launch>

<rosparam param="ticks_meter">17825</rosparam>
<node pkg="differential_drive" type="diff_tf.py" name="odometry" output="screen">
        <remap from="lwheel" to="left_encoder" />
        <remap from="rwheel" to="right_encoder" />
        <rosparam param="base_width">0.195</rosparam>
        <rosparam param="odom_frame_id" subst_value="True" > "/odom" </rosparam>
        <rosparam param="base_frame_id" subst_value="True"> "/base_link" </rosparam>
        <rosparam param="global_frame_id" subst_value="True"> "/map" </rosparam>
        <rosparam param="rate">50</rosparam>
</node>
<node pkg="differential_drive" type="pid_velocity.py" name="lpid">
        <remap from="wheel" to= "left_encoder"/>
        <remap from="motor_cmd" to= "left_pwm"/>
        <remap from="wheel_vtarget" to= "left_target"/>
        <remap from="wheel_vel" to= "left_actual"/>
        <rosparam param="Kp">480</rosparam >
        <rosparam param="Ki">250</rosparam >
        <rosparam param="Kd">18</rosparam >
        <rosparam param="out_min">-255</rosparam >
        <rosparam param="out_max">255</rosparam >
        <rosparam param="rate">40</rosparam >
        <rosparam param="timeout_ticks">4</rosparam>
        <rosparam param="rolling_pts">5</rosparam>
</node>
<node pkg="differential_drive" type="pid_velocity.py" name="rpid">
        <remap from="wheel" to="right_encoder"/>
        <remap from="motor_cmd" to="right_pwm"/>
        <remap from="wheel_vtarget" to="right_target"/>
        <remap from="wheel_vel" to="right_actual"/>
        <rosparam param="Kp">480</rosparam>
        <rosparam param="Ki">250</rosparam>
        <rosparam param="Kd">18</rosparam>
        <rosparam param="out_min">-255</rosparam>
        <rosparam param="out_max">255</rosparam>
        <rosparam param="rate">40</rosparam>
        <rosparam param="timeout_ticks">4</rosparam>
        <rosparam param="rolling_pts">5</rosparam>
</node>
<node pkg="differential_drive" type="twist_to_motors.py" name="twist" output="screen">
        <remap from="twist" to="cmd_vel"/>
        <remap from="lwheel_vtarget" to="left_target"/> ...