diff --git a/src/ezbot_robot/CMakeLists.txt b/src/ezbot_robot/CMakeLists.txt
index 993f9d8..5b23c17 100644
--- a/src/ezbot_robot/CMakeLists.txt
+++ b/src/ezbot_robot/CMakeLists.txt
@@ -33,13 +33,13 @@ if(BUILD_TESTING)
 endif()
 
 install(
-  DIRECTORY config description launch worlds meshes media
+  DIRECTORY config description launch worlds meshes media maps
   DESTINATION share/${PROJECT_NAME}
 )
 
 install(
-  DIRECTORY media/
-  DESTINATION $ENV{HOME}/.gazebo/media/
+  DIRECTORY maps/
+  DESTINATION $ENV{HOME}/ros2maps/
 )
 
 install(
@@ -47,6 +47,8 @@ install(
   DESTINATION $ENV{HOME}/.gazebo/media/
 )
 
+
+
 install(PROGRAMS
   launch/camerapubsub.py
   DESTINATION lib/${PROJECT_NAME})
diff --git a/src/ezbot_robot/config/nav2_params.yaml b/src/ezbot_robot/config/nav2_params.yaml
index b373900..1453901 100644
--- a/src/ezbot_robot/config/nav2_params.yaml
+++ b/src/ezbot_robot/config/nav2_params.yaml
@@ -69,11 +69,11 @@ controller_server:
   ros__parameters:
     controller_frequency: 20.0
     min_x_velocity_threshold: 0.001
-    min_y_velocity_threshold: 0.5
+    min_y_velocity_threshold: 0.001
     min_theta_velocity_threshold: 0.001
     failure_tolerance: 0.3
     progress_checker_plugins: ["progress_checker"]
-    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+    goal_checker_plugins: ["precise_goal_checker"] # "precise_goal_checker"
     controller_plugins: ["FollowPath"]
     use_realtime_priority: false
 
@@ -83,11 +83,11 @@ controller_server:
       required_movement_radius: 0.5
       movement_time_allowance: 10.0
     # Goal checker parameters
-    #precise_goal_checker:
-    #  plugin: "nav2_controller::SimpleGoalChecker"
-    #  xy_goal_tolerance: 0.25
-    #  yaw_goal_tolerance: 0.25
-    #  stateful: True
+    precise_goal_checker:
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.02
+      yaw_goal_tolerance: 0.02
+      stateful: True
     general_goal_checker:
       stateful: True
       plugin: "nav2_controller::SimpleGoalChecker"
diff --git a/src/ezbot_robot/description/gazebo_control.xacro b/src/ezbot_robot/description/gazebo_control.xacro
index 8aa9391..5aa2531 100644
--- a/src/ezbot_robot/description/gazebo_control.xacro
+++ b/src/ezbot_robot/description/gazebo_control.xacro
@@ -7,6 +7,7 @@
             <West>roue_gauche_joint</West>
             <South>roue_arriere_joint</South>
             <East>roue_droite_joint</East>
+            <commandTopic>/omnidirectional_controller/cmd_vel_unstamped</commandTopic>
             
             <wheel_separation>0.25</wheel_separation>
             <wheel_diameter>0.06</wheel_diameter>
diff --git a/src/ezbot_robot/description/lidar.xacro b/src/ezbot_robot/description/lidar.xacro
index e66db3c..fd32c92 100644
--- a/src/ezbot_robot/description/lidar.xacro
+++ b/src/ezbot_robot/description/lidar.xacro
@@ -57,7 +57,7 @@
 
         <sensor name="laser" type="ray">
             <pose> 0.055 0 0.03 0 0 0 </pose>
-            <visualize>false</visualize>
+            <visualize>true</visualize>
             <update_rate>10</update_rate>
             <ray>
                 <scan>
@@ -74,7 +74,7 @@
             </ray>
             <plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
                 <ros>
-                    <argument>~/out:=scan</argument>
+                    <argument>~/out:=lidar/ldlidar_node/scan</argument>
                 </ros>
                 <output_type>sensor_msgs/LaserScan</output_type>
                 <frame_name>base_laser</frame_name>
diff --git a/src/ezbot_robot/description/robot.urdf.xacro b/src/ezbot_robot/description/robot.urdf.xacro
index 3a1fae4..232aa38 100644
--- a/src/ezbot_robot/description/robot.urdf.xacro
+++ b/src/ezbot_robot/description/robot.urdf.xacro
@@ -4,9 +4,7 @@
     <xacro:include filename="robot_core.xacro" />
     <xacro:include filename="lidar.xacro"/>
     <xacro:include filename="ros2_control.xacro" />
-    <!--
-        <xacro:include filename="gazebo_control.xacro" />
-    -->
+    <xacro:include filename="gazebo_control.xacro" />
     <xacro:include filename="camera.xacro" />
     <xacro:include filename="imu.xacro" />
 </robot>
\ No newline at end of file
diff --git a/src/ezbot_robot/launch/map_server.launch.py b/src/ezbot_robot/launch/map_server.launch.py
new file mode 100644
index 0000000..586c632
--- /dev/null
+++ b/src/ezbot_robot/launch/map_server.launch.py
@@ -0,0 +1,51 @@
+import os
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
+from launch.substitutions import LaunchConfiguration
+import launch_ros.actions
+
+
+def generate_launch_description():
+
+    ld = LaunchDescription()
+
+    # map file
+    map_file_path = os.path.join(
+        get_package_share_directory('ezbot_robot'),
+        'maps',
+        'testMap',
+        'testMap.yaml'
+    )
+
+    map_server_cmd = Node(
+        package='nav2_map_server',
+        executable='map_server',
+        output='screen',
+        namespace='map',
+        parameters=[{'yaml_filename': map_file_path},
+                    {'frame_id': 'map'}],
+        remappings=[('/map/map', '/map')]
+    )
+
+
+    lifecycle_nodes = ['map_server']
+    autostart = True
+
+    start_lifecycle_manager_cmd = launch_ros.actions.Node(
+            package='nav2_lifecycle_manager',
+            executable='lifecycle_manager',
+            name='lifecycle_manager',
+            namespace='map',
+            output='screen',
+            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+            parameters=[{'autostart': autostart},
+                        {'node_names': lifecycle_nodes}])
+
+
+    ld.add_action(map_server_cmd)
+    ld.add_action(start_lifecycle_manager_cmd)
+
+    return ld
diff --git a/src/ezbot_robot/maps/testMap/blackandwhite.jpg b/src/ezbot_robot/maps/testMap/blackandwhite.jpg
new file mode 100644
index 0000000..8e6db41
Binary files /dev/null and b/src/ezbot_robot/maps/testMap/blackandwhite.jpg differ
diff --git a/src/ezbot_robot/maps/testMap/blackandwhite.pgm b/src/ezbot_robot/maps/testMap/blackandwhite.pgm
new file mode 100644
index 0000000..212521d
Binary files /dev/null and b/src/ezbot_robot/maps/testMap/blackandwhite.pgm differ
diff --git a/src/ezbot_robot/maps/testMap/testMap.yaml b/src/ezbot_robot/maps/testMap/testMap.yaml
new file mode 100644
index 0000000..16693fa
--- /dev/null
+++ b/src/ezbot_robot/maps/testMap/testMap.yaml
@@ -0,0 +1,7 @@
+yaml_filename: "yaml_filename"
+image: blackandwhite.jpg
+resolution: 0.0100
+origin: [1.0, 2.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196