I have implemented the "waypoint_nav" package from "https://github.com/nickcharron/waypoi..." 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 "navsat_params.yaml": navsat_transform: frequency: 30 delay: 3.0 magnetic_declination_radians: 0.02337 yaw_offset: 1.57080 zero_altitude: true broadcast_utm_transform: true publish_filtered_gps: true use_odometry_yaw: false wait_for_datum: false

file "ekf_params.yaml":

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

ekf_se_odom: # Used only for broadcasting odom to base_link transforms frequency: 30 sensor_timeout: 0.1 two_d_mode: true transform_time_offset: 0.0 transform_timeout: 0.0 print_diagnostics: true debug: false

map_frame: map odom_frame: odom base_link_frame: base_link world_frame: odom

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

Wheel odometry:

odom0: /husky_velocity_controller/odom odom0_config: [false, false, false, false, false, false, true, true, true, false, false, false, false, false, false] odom0_queue_size: 10 odom0_nodelay: true odom0_differential: false odom0_relative: false

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

Laser scanmatching odometry:

odom1: scanmatch_odom

odom1_config: [false, false, false,

false, false, false,

true, true, true,

false, false, true,

false, false, false]

odom1_queue_size: 10

odom1_nodelay: true

odom1_differential: false

odom1_relative: false

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

imu configure:

imu0: /imu/data imu0_config: [false, false, false, true, true, false, false, false, false, true, true, true, true, true, true] imu0_nodelay: false imu0_differential: false imu0_relative: false imu0_queue_size: 10 imu0_remove_gravitational_acceleration: true

use_control: false

process_noise_covariance: [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]

initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0 ... (more)