diff --git a/Makefile b/Makefile
index d461513..780e5c8 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,9 @@
 docker_start:
 	@./scripts/run_dev.sh 
 
+nav2:
+	@cd deploy && docker build --no-cache --tag go2py_nav2:latest -f docker/Dockerfile.nav2 .
+
 messages:
 	@cd scripts && ./make_msgs.sh 
 
diff --git a/deploy/docker/Dockerfile.nav2 b/deploy/docker/Dockerfile.nav2
index bef1981..dfc9ad1 100644
--- a/deploy/docker/Dockerfile.nav2
+++ b/deploy/docker/Dockerfile.nav2
@@ -11,6 +11,8 @@ RUN apt-get update && apt-get install -y \
     cmake \
     git \
     ros-humble-nav2-* ros-humble-navigation2 \
+    ros-humble-slam-toolbox \ 
+    ros-humble-robot-localization\
     && rm -rf /var/lib/apt/lists/*
 
 # Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree
@@ -19,12 +21,9 @@ git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://
 cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
 
 # copy the go2py ros2 nodes
-#COPY ros2_nodes/lidar_node /hesai_ws/src/lidar_node
-#RUN cd /hesai_ws && source /opt/ros/humble/setup.bash && colcon build --symlink-install
+COPY ros2_nodes/sportmode_nav2 /nav2_ws/src/sportmode_nav2
+COPY ros2_nodes/go2py_messages /nav2_ws/src/go2py_messages
+RUN cd /nav2_ws && source /opt/ros/humble/setup.bash && source /unitree_ros2/cyclonedds_ws/install/setup.bash && colcon build --symlink-install
 
-# Copy the script to start the nodes
-#COPY docker/scripts /root/scripts
-#COPY launch_files /root/launch
 # set the entrypoint to bash
 ENTRYPOINT ["/bin/bash"]
-#ENTRYPOINT ["/bin/bash", "/root/scripts/hesai_start.sh"]
diff --git a/deploy/launch_files/robot_description.launch.py b/deploy/launch_files/robot_description.launch.py
index 6817531..c4aa683 100644
--- a/deploy/launch_files/robot_description.launch.py
+++ b/deploy/launch_files/robot_description.launch.py
@@ -28,20 +28,4 @@ def generate_launch_description():
                     ("/robot_description", "/go2/robot_description"),
             ],
         ),
-        Node(
-            package="tf2_ros",
-            executable="static_transform_publisher",
-            arguments=[
-                    "0.15",
-                    "0",
-                    "0.15",
-                    "0",
-                    "0",
-                    "0.707107",
-                    "0.707107",
-                    "/trunk",
-                    "/go2/hesai_lidar",
-            ],
-            name="static_tf_pub_trunk_to_lidar",
-        ),
     ])
diff --git a/deploy/ros2_nodes/go2_description/xacro/robot.xacro b/deploy/ros2_nodes/go2_description/xacro/robot.xacro
index 8494b8d..327cc87 100755
--- a/deploy/ros2_nodes/go2_description/xacro/robot.xacro
+++ b/deploy/ros2_nodes/go2_description/xacro/robot.xacro
@@ -170,6 +170,20 @@
         <child link="utlidar"/>
         <axis xyz="0 0 0"/>
     </joint>
+
+    <link name="hesai">
+        <inertial>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <mass value="0"/>
+        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+        </inertial>
+    </link>
+    <joint name="hesai_joint" type="fixed">
+        <origin rpy="0 0 1.57075" xyz="0.15 0 0.25"/>
+        <parent link="base_link"/>
+        <child link="hesai"/>
+        <axis xyz="0 0 0"/>
+    </joint>
     
     <xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1"  front_hind_dae="True"  />
     <xacro:leg name="FL" mirror="1"  mirror_dae="True"   front_hind="1"  front_hind_dae="True"  />
diff --git a/deploy/ros2_nodes/lidar_node/config/config.yaml b/deploy/ros2_nodes/lidar_node/config/config.yaml
index 9ed5a52..3e5473e 100644
--- a/deploy/ros2_nodes/lidar_node/config/config.yaml
+++ b/deploy/ros2_nodes/lidar_node/config/config.yaml
@@ -18,9 +18,9 @@ lidar:
       pitch: 0
       yaw: 0
     ros:
-      ros_frame_id: go2/hesai_lidar                      #Frame id of packet message and point cloud message
+      ros_frame_id: hesai                              #Frame id of packet message and point cloud message
       ros_recv_packet_topic: go2/lidar_packets         #Topic used to receive lidar packets from rosbag
       ros_send_packet_topic: go2/lidar_packets         #Topic used to send lidar raw packets through ROS
       ros_send_point_cloud_topic: go2/lidar_points     #Topic used to send point cloud through ROS
-      send_packet_ros: false                          #true: Send packets through ROS 
-      send_point_cloud_ros: true                     #true: Send point cloud through ROS    
+      send_packet_ros: false                           #true: Send packets through ROS 
+      send_point_cloud_ros: true                       #true: Send point cloud through ROS    
diff --git a/deploy/ros2_nodes/sportmode_nav2/params/ros_ekf.yaml b/deploy/ros2_nodes/sportmode_nav2/params/ros_ekf.yaml
index b72e643..e6010dd 100644
--- a/deploy/ros2_nodes/sportmode_nav2/params/ros_ekf.yaml
+++ b/deploy/ros2_nodes/sportmode_nav2/params/ros_ekf.yaml
@@ -66,17 +66,17 @@ ekf_filter_node:
 #     3a. Set your "world_frame" to your map_frame value
 #     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
 #         estimation node from robot_localization! However, that instance should *not* fuse the global data.
-        map_frame: odom              # Defaults to "map" if unspecified
+        # map_frame: odom              # Defaults to "map" if unspecified
         odom_frame: odom            # Defaults to "odom" if unspecified
         base_link_frame: base_link  # Defaults to "base_link" if unspecified
-        world_frame: odom           # Defaults to the value of odom_frame if unspecified
+        # world_frame: odom           # Defaults to the value of odom_frame if unspecified
 
 # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
 # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
 # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
 # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
 # default values, and must be specified.
-        odom0: /utlidar/robot_odom
+        odom0: /go2/odom
 
 # Each sensor reading updates some or all of the filter's state. These options give you greater control over which
 # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only