Required Info:

• Operating System:
  • ubuntu 20.04
• ROS2 Version:
  • Foxy <!-- ROS2 distribution and install method (e.g. Foxy binaries, Dashing source...) -->
• Version or commit hash:
  • latest binary <!-- from source: output of `git -C navigation2 rev-parse HEAD apt binaries: output of: dpkg-query --show "ros-$ROSDISTRO-navigation2" or: dpkg-query --show "ros-$ROSDISTRO-nav2-*" -->
• DDS implementation:
  • FASTDDS <!-- rmw_implementation used (e.g. Fast-RTPS, RTI Connext, etc.) -->

Steps to reproduce issue

```

robot bringup in robot machine

ros2 launch ros2diffdrive-robot_bringup bringup.launch.py

robot visualization in Host machine

ros2 launch ros2diffdriverobotvisualization robot_model.launch.py

slam toolbox for mapping

ros2 launch ros2diffdriverobotnavigation slam.launch.py ```

Expected behavior

While manually controlling the robot it should map the room as clean as possible and should not jump to a different position

Actual behavior

The map is very noisy as seen in the video. The maps rotate as well with the base_frame which I am not sure it supposed to do it or not. After moving the robot it seems it jumps to a different position which is making the map unusable

1. video with SLAM toolbox - video link
   • Note: Imu topic freezes at some point which has been fixed later using a separate power source for IMU. But this is not causing the main issue.
2. video without slam but only lidar laser scan - video link

Additional information

This is a custom differential robot that has teensy 3.2 as a microcontroller running microros client. The robot has also MPU9250 imu and 2 encoders in both motors. For lidar source, I am using YDLIDAR. As seen in the videos the map shifts while manually moving the robot, which is why I cannot get a clean map. The weird jumps or map shift does not appear in the simulation with the same robot description file. Any help is appreciated.

All the config files and launchers are below:

• ekf config file

  ekf_filter_node:
  ros__parameters:
  frequency: 50.0
  two_d_mode: true
  publish_tf: true
  
  map_frame: map             
  odom_frame: odom            
  base_link_frame: base_footprint
  world_frame: odom 
  transform_time_offset: 0.1
  transform_timeout: 0.1
  
  #x     , y     , z,
  #roll  , pitch , yaw,
  #vx    , vy    , vz,
  #vroll , vpitch, vyaw,
  #ax    , ay    , az
  odom0: odom/unfiltered
  odom0_config: [false, false, false,
                 false, false, false,
                 true, true, false,
                 false, false, true,
                 false, false, false]
  
  imu0: imu/data
  imu0_config: [false, false, false,
                false, false, false,
                false, false, false,
                false, false, true,
                false, false, false]
  

• YDLIDAR config

   ydlidar_ros2_driver_node:
   ros__parameters:
   port: /dev/ydlidar
   frame_id: laser
   ignore_array: ""
   baudrate: 128000
   lidar_type: 1
   device_type: 6
   sample_rate: 5
   abnormal_check_count: 4
   resolution_fixed: true
   reversion: true
   inverted: true
   auto_reconnect: true
   isSingleChannel: false
   intensity: false
   support_motor_dtr: true
   angle_max: 180.0
   angle_min: -180.0
   range_max: 10.0
   range_min: 0.12
   frequency: 5.0
   invalid_range_is_inf: false
   transform_time_offset: 0.1
  transform_timeout: 0.1
  

• bringup.launch.py

   import os
   import yaml
   from ament_index_python.packages import get_package_share_directory
   from launch import LaunchDescription
   from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
   from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
   from launch.launch_description_sources import PythonLaunchDescriptionSource
   from launch_ros.actions import Node
   from launch_ros.substitutions import FindPackageShare
   from launch.conditions import IfCondition
  
   def generate_launch_description():
   sensors_launch_path = PathJoinSubstitution(
    [FindPackageShare('ros2_diff_drive_robot_bringup'), 'launch', 'sensors.launch.py']
    )
  
   joy_launch_path = PathJoinSubstitution(
   [FindPackageShare('ros2_diff_drive_robot_bringup'), 'launch', 'joy_teleop.launch.py']
   )
  
   description_launch_path = PathJoinSubstitution(
  [FindPackageShare('ros2_diff_drive_robot_description'), 'launch', 'description.launch.py']
   )
  
  #ekf_config_path = PathJoinSubstitution(
  #    [FindPackageShare('ros2_diff_drive_robot_bringup'), 'config', 'ekf.yaml']
   #)
  ekf_config_path = os.path.join(get_package_share_directory('ros2_diff_drive_robot_bringup'), 'config/ekf.yaml')
  
   # Load the parameters specific to your ComposableNode
   with open(ekf_config_path, 'r') as file:
  configParams = yaml.safe_load(file)['ekf_filter_node']['ros__parameters']
  
   return LaunchDescription([
   DeclareLaunchArgument(
      name='base_serial_port', 
      default_value='/dev/ttyACM0',
      description='ros2_diff_drive_robot Base Serial Port'
     ),
   DeclareLaunchArgument(
      name='joy', 
      default_value='false',
      description='Use Joystick'
  ),
  
  Node(
      package='micro_ros_agent',
      executable='micro_ros_agent',
      name='micro_ros_agent',
      output='screen',
      arguments=['serial', '--dev', LaunchConfiguration("base_serial_port")]
  ),
  
  Node(
      package='robot_localization',
      executable='ekf_node',
      name='ekf_filter_node',
      output='screen',
      parameters=[configParams]
      #remappings=[("odometry/filtered", "odom")]
  
  ),
  
  #Node(package = "tf2_ros", 
  #     executable = "static_transform_publisher",
  #     output='screen',
  #     arguments = ["0", "0", "0", "0", "0", "0", "map", "base_footprint"]
  #),
  
  IncludeLaunchDescription(
      PythonLaunchDescriptionSource(description_launch_path)
  ),
  
  IncludeLaunchDescription(
      PythonLaunchDescriptionSource(sensors_launch_path),
  ),
  
  IncludeLaunchDescription(
      PythonLaunchDescriptionSource(joy_launch_path),
      condition=IfCondition(LaunchConfiguration("joy")),
  )
  

  ])

• Robot Description file based on linorobot2

  • Link: Description file

• Navigation config file

      amcl:
  ros__parameters:
  set_initial_pose: True
  initial_pose.x: 0.0
  initial_pose.y: 0.0
  use_sim_time: False
  alpha1: 0.2
  alpha2: 0.2
  alpha3: 0.2
  alpha4: 0.2
  alpha5: 0.2
  base_frame_id: "base_footprint"
  beam_skip_distance: 0.5
  beam_skip_error_threshold: 0.9
  beam_skip_threshold: 0.3
  do_beamskip: false
  global_frame_id: "map"
  lambda_short: 0.1
  laser_likelihood_max_dist: 2.0
  laser_max_range: 100.0
  laser_min_range: -1.0
  laser_model_type: "likelihood_field"
  max_beams: 60
  max_particles: 2000
  min_particles: 500
  odom_frame_id: "odom"
  pf_err: 0.05
  pf_z: 0.99
  recovery_alpha_fast: 0.0
  recovery_alpha_slow: 0.0
  resample_interval: 1
  robot_model_type: "differential"
  save_pose_rate: 0.5
  sigma_hit: 0.2
  tf_broadcast: true
  transform_tolerance: 1.0
  update_min_a: 0.2
  update_min_d: 0.25
  z_hit: 0.5
  z_max: 0.05
  z_rand: 0.5
  z_short: 0.05
  scan_topic: scan
  
  amcl_map_client:
  ros__parameters:
  use_sim_time: False
  
  amcl_rclcpp_node:
  ros__parameters:
  use_sim_time: False
  
  bt_navigator:
  ros__parameters:
  use_sim_time: False
  global_frame: map
  robot_base_frame: base_link
  odom_topic: /odom
  enable_groot_monitoring: True
  groot_zmq_publisher_port: 1666
  groot_zmq_server_port: 1667
  default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
  plugin_lib_names:
  - nav2_compute_path_to_pose_action_bt_node
  - nav2_follow_path_action_bt_node
  - nav2_back_up_action_bt_node
  - nav2_spin_action_bt_node
  - nav2_wait_action_bt_node
  - nav2_clear_costmap_service_bt_node
  - nav2_is_stuck_condition_bt_node
  - nav2_goal_reached_condition_bt_node
  - nav2_goal_updated_condition_bt_node
  - nav2_initial_pose_received_condition_bt_node
  - nav2_reinitialize_global_localization_service_bt_node
  - nav2_rate_controller_bt_node
  - nav2_distance_controller_bt_node
  - nav2_speed_controller_bt_node
  - nav2_truncate_path_action_bt_node
  - nav2_goal_updater_node_bt_node
  - nav2_recovery_node_bt_node
  - nav2_pipeline_sequence_bt_node
  - nav2_round_robin_node_bt_node
  - nav2_transform_available_condition_bt_node
  - nav2_time_expired_condition_bt_node
  - nav2_distance_traveled_condition_bt_node
  
  bt_navigator_rclcpp_node:
  ros__parameters:
  use_sim_time: False
  
  controller_server:
  ros__parameters:
  use_sim_time: False
  controller_frequency: 20.0
  min_x_velocity_threshold: 0.001
  min_y_velocity_threshold: 0.5
  min_theta_velocity_threshold: 0.001
  progress_checker_plugin: "progress_checker"
  goal_checker_plugin: "goal_checker"
  controller_plugins: ["FollowPath"]
  
  # Progress checker parameters
  progress_checker:
  plugin: "nav2_controller::SimpleProgressChecker"
  required_movement_radius: 0.5
  movement_time_allowance: 10.0
  # Goal checker parameters
  goal_checker:
  plugin: "nav2_controller::SimpleGoalChecker"
  xy_goal_tolerance: 0.25
  yaw_goal_tolerance: 0.25
  stateful: True
  # DWB parameters
  FollowPath:
  plugin: "dwb_core::DWBLocalPlanner"
  debug_trajectory_details: True
  min_vel_x: 0.0
  min_vel_y: 0.0
  max_vel_x: 0.35
  max_vel_y: 0.0
  max_vel_theta: 1.0
  min_speed_xy: 0.0
  max_speed_xy: 0.35
  min_speed_theta: 0.0
  # Add high threshold velocity for turtlebot 3 issue.
  # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
  acc_lim_x: 2.5
  acc_lim_y: 0.0
  acc_lim_theta: 3.2
  decel_lim_x: -2.5
  decel_lim_y: 0.0
  decel_lim_theta: -3.2
  vx_samples: 20
  vy_samples: 5
  vtheta_samples: 20
  sim_time: 1.7
  linear_granularity: 0.05
  angular_granularity: 0.025
  transform_tolerance: 0.2
  xy_goal_tolerance: 0.25
  trans_stopped_velocity: 0.25
  short_circuit_trajectory_evaluation: True
  stateful: True
  critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
  BaseObstacle.scale: 0.02
  PathAlign.scale: 32.0
  PathAlign.forward_point_distance: 0.1
  GoalAlign.scale: 24.0
  GoalAlign.forward_point_distance: 0.1
  PathDist.scale: 32.0
  GoalDist.scale: 24.0
  RotateToGoal.scale: 32.0
  RotateToGoal.slowing_factor: 5.0
  RotateToGoal.lookahead_time: -1.0
  
  controller_server_rclcpp_node:
  ros__parameters:
  use_sim_time: False
  
  local_costmap:
  local_costmap:
  ros__parameters:
  update_frequency: 5.0
  publish_frequency: 2.0
  global_frame: odom
  robot_base_frame: base_link
  use_sim_time: False
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.05
  robot_radius: 0.22
  plugins: ["voxel2d_layer", "voxel3d_layer", "inflation_layer"]
  inflation_layer:
  plugin: "nav2_costmap_2d::InflationLayer"
  cost_scaling_factor: 3.0
  inflation_radius: 0.55
  voxel2d_layer:
  plugin: "nav2_costmap_2d::VoxelLayer"
  enabled: True
  publish_voxel_map: False
  origin_z: 0.0
  z_resolution: 0.05
  z_voxels: 16
  max_obstacle_height: 2.0
  mark_threshold: 0
  observation_sources: scan base_scan
  scan:
  topic: /scan
  max_obstacle_height: 2.0
  clearing: True
  marking: True
  data_type: "LaserScan"
  base_scan:
  topic: /base/scan
  max_obstacle_height: 2.0
  clearing: True
  marking: True
  data_type: "LaserScan"
  voxel3d_layer:
  plugin: "nav2_costmap_2d::VoxelLayer"
  enabled: True
  publish_voxel_map: False
  origin_z: 0.0
  z_resolution: 0.05
  z_voxels: 16
  mark_threshold: 1
  observation_sources: realsense zed
  min_obstacle_height: 0.00
  max_obstacle_height: 2.0 
  realsense:
  topic: /camera/depth/color/points
  raytrace_min_range: 0.5
  min_obstacle_height: 0.0
  max_obstacle_height: 2.0 
  clearing: True
  marking: True
  data_type: "PointCloud2"
  zed:
  topic: /zed/point_cloud/cloud_registered
  raytrace_min_range: 0.5
  min_obstacle_height: 0.0
  max_obstacle_height: 2.0 
  clearing: True
  marking: True
  data_type: "PointCloud2"
  static_layer:
  map_subscribe_transient_local: True
  always_send_full_costmap: True
  local_costmap_client:
  ros__parameters:
  use_sim_time: False
  local_costmap_rclcpp_node:
  ros__parameters:
  use_sim_time: False
  
  global_costmap:
  global_costmap:
  ros__parameters:
  update_frequency: 5.0
  publish_frequency: 1.0
  global_frame: map
  robot_base_frame: base_link
  use_sim_time: False
  robot_radius: 0.22
  resolution: 0.05
  track_unknown_space: true
  plugins: ["static_layer", "voxel2d_layer", "voxel3d_layer", "inflation_layer"]
  voxel2d_layer:
  plugin: "nav2_costmap_2d::ObstacleLayer"
  enabled: True
  observation_sources: scan base_scan
  scan:
  topic: /scan
  max_obstacle_height: 2.0
  clearing: True
  marking: True
  data_type: "LaserScan"
  base_scan:
  topic: /base/scan
  max_obstacle_height: 2.0
  clearing: True
  marking: True
  data_type: "LaserScan"
  voxel3d_layer:
  plugin: "nav2_costmap_2d::VoxelLayer"
  enabled: True
  publish_voxel_map: False
  origin_z: 0.0
  z_resolution: 0.05
  z_voxels: 16
  mark_threshold: 1
  observation_sources: realsense zed
  min_obstacle_height: 0.00
  max_obstacle_height: 2.0 
  realsense:
  topic: /camera/depth/color/points
  raytrace_min_range: 0.5
  min_obstacle_height: 0.0
  max_obstacle_height: 2.0 
  clearing: True
  marking: True
  data_type: "PointCloud2"
  zed:
  topic: /zed/point_cloud/cloud_registered
  raytrace_min_range: 0.5
  min_obstacle_height: 0.0
  max_obstacle_height: 2.0 
  clearing: True
  marking: True
  data_type: "PointCloud2"
  static_layer:
  plugin: "nav2_costmap_2d::StaticLayer"
  map_subscribe_transient_local: True
  inflation_layer:
  plugin: "nav2_costmap_2d::InflationLayer"
  cost_scaling_factor: 3.0
  inflation_radius: 0.55
  always_send_full_costmap: True
  global_costmap_client:
  ros__parameters:
  use_sim_time: False
  global_costmap_rclcpp_node:
  ros__parameters:
  use_sim_time: False
  
  map_server:
  ros__parameters:
  use_sim_time: False
  yaml_filename: "playground.yaml"
  
  map_saver:
  ros__parameters:
  use_sim_time: False
  save_map_timeout: 5000
  free_thresh_default: 0.25
  occupied_thresh_default: 0.65
  map_subscribe_transient_local: False
  
  planner_server:
  ros__parameters:
  expected_planner_frequency: 20.0
  use_sim_time: False
  planner_plugins: ["GridBased"]
  GridBased:
  plugin: "nav2_navfn_planner/NavfnPlanner"
  tolerance: 0.5
  use_astar: false
  allow_unknown: true
  
  planner_server_rclcpp_node:
  ros__parameters:
  use_sim_time: False
  
  recoveries_server:
  ros__parameters:
  costmap_topic: local_costmap/costmap_raw
  footprint_topic: local_costmap/published_footprint
  cycle_frequency: 10.0
  recovery_plugins: ["spin", "back_up", "wait"]
  spin:
  plugin: "nav2_recoveries/Spin"
  back_up:
  plugin: "nav2_recoveries/BackUp"
  wait:
  plugin: "nav2_recoveries/Wait"
  global_frame: odom
  robot_base_frame: base_link
  transform_timeout: 0.1
  use_sim_time: False
  simulate_ahead_time: 2.0
  max_rotational_vel: 1.0
  min_rotational_vel: 0.4
  rotational_acc_lim: 3.2
  
  robot_state_publisher:
  ros__parameters:
  use_sim_time: False
  

• SLAM config yaml file

      slam_toolbox:
  ros__parameters:
  
  # Plugin params
  solver_plugin: solver_plugins::CeresSolver
  ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
  ceres_preconditioner: SCHUR_JACOBI
  ceres_trust_strategy: LEVENBERG_MARQUARDT
  ceres_dogleg_type: TRADITIONAL_DOGLEG
  ceres_loss_function: None
  
  # ROS Parameters
  odom_frame: odom
  map_frame: map
  base_frame: base_footprint
  scan_topic: /scan
  mode: mapping #localization
  
  # if you'd like to immediately start continuing a map at a given pose
  # or at the dock, but they are mutually exclusive, if pose is given
  # will use pose
  #map_file_name: test_steve
  # map_start_pose: [0.0, 0.0, 0.0]
  #map_start_at_dock: true
  
  debug_logging: true
  throttle_scans: 1
  transform_publish_period: 0.02 #if 0 never publishes odometry
  map_update_interval: 5.0
  resolution: 0.05
  max_laser_range: 10.0 #for rastering images
  minimum_time_interval: 0.5
  transform_timeout: 0.2
  tf_buffer_duration: 30.
  stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
  enable_interactive_mode: true
  
  # General Parameters
  use_scan_matching: true
  use_scan_barycenter: true
  minimum_travel_distance: 0.5
  minimum_travel_heading: 0.5
  scan_buffer_size: 10
  scan_buffer_maximum_scan_distance: 10.0
  link_match_minimum_response_fine: 0.1  
  link_scan_maximum_distance: 1.5
  loop_search_maximum_distance: 3.0
  do_loop_closing: true 
  loop_match_minimum_chain_size: 10           
  loop_match_maximum_variance_coarse: 3.0  
  loop_match_minimum_response_coarse: 0.35    
  loop_match_minimum_response_fine: 0.45
  
  # Correlation Parameters - Correlation Parameters
  correlation_search_space_dimension: 0.5
  correlation_search_space_resolution: 0.01
  correlation_search_space_smear_deviation: 0.1 
  
  # Correlation Parameters - Loop Closure Parameters
  loop_search_space_dimension: 8.0
  loop_search_space_resolution: 0.05
  loop_search_space_smear_deviation: 0.03
  
  # Scan Matcher Parameters
  distance_variance_penalty: 0.5      
  angle_variance_penalty: 1.0    
  
  fine_search_angle_offset: 0.00349     
  coarse_search_angle_offset: 0.349   
  coarse_angle_resolution: 0.0349        
  minimum_angle_penalty: 0.9
  minimum_distance_penalty: 0.5
  use_response_expansion: true
  

• SLAM launch file

  import os
  from launch import LaunchDescription
  from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
  from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
  from launch.launch_description_sources import PythonLaunchDescriptionSource
  from launch.conditions import IfCondition
  from launch.substitutions import EnvironmentVariable
  from launch_ros.substitutions import FindPackageShare
  from launch_ros.actions import Node
  
  def generate_launch_description():
  slam_launch_path = PathJoinSubstitution(
  [FindPackageShare('slam_toolbox'), 'launch', 'online_async_launch.py']
  )
  
  slam_config_path = PathJoinSubstitution(
  [FindPackageShare('ros2_diff_drive_robot_navigation'), 'config', 'slam.yaml']
  )
  
  rviz_config_path = PathJoinSubstitution(
  [FindPackageShare('ros2_diff_drive_robot_navigation'), 'rviz', 'ros2_diff_drive_robot_slam.rviz']
  )
  
  ros_distro = EnvironmentVariable('ROS_DISTRO')
  slam_param_name = 'params_file'
  if ros_distro == 'foxy': 
  slam_param_name = 'slam_params_file'
  
  return LaunchDescription([
  DeclareLaunchArgument(
  name='sim', 
  default_value='false',
  description='Enable use_sime_time to true'
  ),
  
  DeclareLaunchArgument(
  name='rviz', 
  default_value='false',
  description='Run rviz'
  ),
  
  IncludeLaunchDescription(
  PythonLaunchDescriptionSource(slam_launch_path),
  launch_arguments={
      'use_sim_time': LaunchConfiguration("sim"),
      slam_param_name: slam_config_path
  }.items()
  ),
  
  Node(
  package='rviz2',
  executable='rviz2',
  name='rviz2',
  output='screen',
  arguments=['-d', rviz_config_path],
  condition=IfCondition(LaunchConfiguration("rviz")),
  parameters=[{'use_sim_time': LaunchConfiguration("sim")}]
  )
  ])