I have implemented the "waypointnav" package from "https://github.com/nickcharron/waypointnav" on the actual Husky A200 robot with Novatel GPS sensor, UM7 IMU sensor, and Lm5xx LIDAR sensor. However, when sending the destination point, the robot does not move correctly. Specifically, the "local cost map" and "global cost map" are skewed and the destination point is shifted accordingly, leading to the robot not precisely following the target destination. Below are some configuration files.

file "navsatparams.yaml": navsattransform: frequency: 30 delay: 3.0 magneticdeclinationradians: 0.02337 yawoffset: 1.57080 zeroaltitude: true broadcastutmtransform: true publishfilteredgps: true useodometryyaw: false waitfordatum: false

file "ekf_params.yaml":

For parameter descriptions, please refer to the template parameter files for each node.

ekfseodom: # Used only for broadcasting odom to baselink transforms frequency: 30 sensortimeout: 0.1 twodmode: true transformtimeoffset: 0.0 transformtimeout: 0.0 printdiagnostics: true debug: false

mapframe: map odomframe: odom baselinkframe: baselink worldframe: odom

-------------------------------------

Wheel odometry:

odom0: /huskyvelocitycontroller/odom odom0config: [false, false, false, false, false, false, true, true, true, false, false, false, false, false, false] odom0queuesize: 10 odom0nodelay: true odom0differential: false odom0relative: false

-------------------------------------

Laser scanmatching odometry:

odom1: scanmatch_odom

odom1_config: [false, false, false,

false, false, false,

true, true, true,

false, false, true,

false, false, false]

odom1queuesize: 10

odom1_nodelay: true

odom1_differential: false

odom1_relative: false

--------------------------------------

imu configure:

imu0: /imu/data imu0config: [false, false, false, true, true, false, false, false, false, true, true, true, true, true, true] imu0nodelay: false imu0differential: false imu0relative: false imu0queuesize: 10 imu0removegravitational_acceleration: true

use_control: false

processnoisecovariance: [1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3]

initialestimatecovariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0]

ekfsemap: frequency: 30 sensortimeout: 0.1 twodmode: true transformtimeoffset: 0.0 transformtimeout: 0.0 print_diagnostics: true debug: false

mapframe: map odomframe: odom baselinkframe: baselink worldframe: map

-------------------------------------

Wheel odometry:

odom0: /huskyvelocitycontroller/odom odom0config: [false, false, false, false, false, false, true, true, true, false, false, true, false, false, false] odom0queuesize: 10 odom0nodelay: true odom0differential: false odom0relative: false

-------------------------------------

GPS odometry:

odom1: /outdoorwaypointnav/odometry/gps odom1config: [true, true, false, false, false, false, false, false, false, false, false, false, false, false, false] odom1queuesize: 10 odom1nodelay: true odom1differential: false odom1relative: false

-------------------------------------

Laser scanmatching odometry:

odom2: scanmatch_odom

odom2_config: [false, false, false,

false, false, false,

true, true, true,

false, false, true,

false, false, false]

odom2queuesize: 10

odom2_nodelay: true

odom2_differential: false

odom2_relative: false

--------------------------------------

imu configure:

imu0: /imu/data imu0config: [false, false, false, true, true, false, false, false, false, true, true, true, true, true, true] imu0nodelay: true imu0differential: false imu0relative: false imu0queuesize: 10 imu0removegravitational_acceleration: true

use_control: false

processnoisecovariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3]

initialestimatecovariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0]

file"rangefilter.yaml": scanfilterchain: - name: range type: LaserScanRangeFilter params: lowerthreshold: 0.0 upper_threshold: 3.0

file"localcostmapparams.yaml": localcostmap: globalframe: odom robotbaseframe: baselink updatefrequency: 1.0 publishfrequency: 2.0 staticmap: false rolling_window: true width: 10.0 height: 10.0 resolution: 0.05

file"costmapcommonparams.yaml":

maptype: costmap originz: 0.0 zresolution: 1 # The z resolution of the map in meters/cell. zvoxels: 2 # The number of voxels to in each vertical column, the height of the grid is z resolution * z voxels.

obstaclerange: 2.5 # The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters. raytracerange: 5.5 # The default range in meters at which to raytrace out obstacles from the map using sensor data

publishvoxelmap: false

footprint: [[0.33, 0.43], [-0.33, 0.43], [-0.33, -0.43], [0.33, -0.43], [0.45, 0]] footprintpadding: 0.2 inflationradius: 0.3 # controls how far away the zero cost point is from the obstacle costscalingfactor: 1 # slope of the cost decay curve with respect to distance from the object. lower makes robot stay further from obstacles

observationsources: laserscan_sensor

laserscansensor: {sensorframe: laserlink, data_type: LaserScan, topic: scan, marking: true, clearing: true}

file"globalcostmapparams.yaml":

globalcostmap: globalframe: odom robotbaseframe: baselink updatefrequency: 20 publishfrequency: 5 width: 30.0 height: 30.0 staticmap: true rolling_window: true