diff --git a/stretch_simulation/rviz/stretch_sim.rviz b/stretch_simulation/rviz/stretch_sim.rviz
index 6b749ae..2e41f55 100644
--- a/stretch_simulation/rviz/stretch_sim.rviz
+++ b/stretch_simulation/rviz/stretch_sim.rviz
@@ -438,7 +438,7 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Best Effort
-        Value: /camera_head/left/image_raw
+        Value: /cameras_head/left/image_raw
       Value: true
       Visibility:
         Grid: true
@@ -483,6 +483,74 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 0; 220; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LidarPointsLeft
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /lidar_points_left
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 180; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: LidarPointsRight
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Best Effort
+        Value: /lidar_points_right
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py
index eb4d1eb..2e4625e 100644
--- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py
+++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py
@@ -657,39 +657,32 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None):
             self.camera_compressed_publishers[camera.name].publish(ros_image_compressed)
 
             if camera.is_depth:
-                # if camera == StretchCameras.cam_gripper_se4_stereo_depth:
-                #     pointcloud_msg = create_pointcloud_rgb_msg(
-                #         camera_info_msg=camera_info,
-                #         rgb_image=camera_data.get_camera_data(
-                #             StretchCameras.cam_gripper_se4_stereo_depth
-                #         ),
-                #         depth_image=frame,
-                #     )
-                # elif camera == StretchCameras.cam_hemilidar_left:
-                #     pointcloud_msg = create_pointcloud_rgb_msg(
-                #         camera_info_msg=camera_info,
-                #         rgb_image=camera_data.get_camera_data(
-                #             StretchCameras.cam_nav_rgb_se4_left, auto_rotate=False
-                #         ),
-                #         depth_image=camera_data.get_camera_data(
-                #             StretchCameras.cam_hemilidar_left, auto_rotate=False
-                #         ),
-                #     )
-                # elif camera == StretchCameras.cam_hemilidar_right:
-                #     pointcloud_msg = create_pointcloud_rgb_msg(
-                #         camera_info_msg=camera_info,
-                #         rgb_image=camera_data.get_camera_data(
-                #             StretchCameras.cam_nav_rgb_se4_right, auto_rotate=False
-                #         ),
-                #         depth_image=camera_data.get_camera_data(
-                #             StretchCameras.cam_hemilidar_right, auto_rotate=False
-                #         ),
-                #     )
-                # else:
-                #     pointcloud_msg = create_pointcloud_msg(camera_info, frame)
                 pointcloud_msg = create_pointcloud_msg(camera_info, frame)
                 self.pointcloud_publishers[camera.name].publish(pointcloud_msg)
 
+        # Publish lidar point clouds
+        try:
+            hesai_pts = self.sim.pull_lidar_points()
+            if isinstance(hesai_pts, dict):
+                left_pts = hesai_pts.get("left")
+                right_pts = hesai_pts.get("right")
+
+                if left_pts is not None and len(left_pts) > 0:
+                    header = Header()
+                    header.frame_id = "base_footprint"
+                    header.stamp = current_time
+                    cloud_msg_left = pc2.create_cloud_xyz32(header, left_pts)
+                    self.lidar_left_pub.publish(cloud_msg_left)
+
+                if right_pts is not None and len(right_pts) > 0:
+                    header = Header()
+                    header.frame_id = "base_footprint"
+                    header.stamp = current_time
+                    cloud_msg_right = pc2.create_cloud_xyz32(header, right_pts)
+                    self.lidar_right_pub.publish(cloud_msg_right)
+        except Exception as e:
+            self.logger.error(f"Error publishing lidar points: {e}")
+
     # CHANGE MODES ################
     def change_mode(self, new_mode, code_to_run=None):
         # self.robot_mode_rwlock.acquire_write()
@@ -1026,6 +1019,23 @@ def ros_setup(self):
             "/scan_filtered",
             qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
         )
+        self.lidar_left_pub = self.create_publisher(
+            PointCloud2,
+            "/lidar_points_left",
+            qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
+        )
+        self.lidar_right_pub = self.create_publisher(
+            PointCloud2,
+            "/lidar_points_right",
+            qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT),
+        )
+
+        active_cameras = [
+            StretchCameras.cam_nav_rgb_se4_center
+            if camera == StretchCameras.cam_nav_rgb_se4_center_low_rez
+            else camera
+            for camera in self.sim._cameras_to_use
+        ]
 
         self.camera_publishers = {
             camera.name: self.create_publisher(
@@ -1035,7 +1045,7 @@ def ros_setup(self):
                     depth=1, reliability=ReliabilityPolicy.BEST_EFFORT
                 ),
             )
-            for camera in self.sim._cameras_to_use
+            for camera in active_cameras
         }
         self.camera_compressed_publishers = {
             camera.name: self.create_publisher(
@@ -1045,7 +1055,7 @@ def ros_setup(self):
                     depth=1, reliability=ReliabilityPolicy.BEST_EFFORT
                 ),
             )
-            for camera in self.sim._cameras_to_use
+            for camera in active_cameras
         }
         self.pointcloud_publishers = {
             camera.name: self.create_publisher(
@@ -1055,7 +1065,7 @@ def ros_setup(self):
                     depth=1, reliability=ReliabilityPolicy.BEST_EFFORT
                 ),
             )
-            for camera in self.sim._cameras_to_use
+            for camera in active_cameras
             if camera.is_depth
         }
         self.camera_info_publishers = {
@@ -1063,10 +1073,10 @@ def ros_setup(self):
                 CameraInfo,
                 get_camera_info_topic_name(camera),
                 qos_profile=QoSProfile(
-                    depth=1, reliability=ReliabilityPolicy.BEST_EFFORT
+                    depth=1, reliability=ReliabilityPolicy.RELIABLE
                 ),
             )
-            for camera in self.sim._cameras_to_use
+            for camera in active_cameras
         }
 
         self.clock_pub = self.create_publisher(
@@ -1396,12 +1406,8 @@ def get_camera_topic_name(camera: StretchCameras):
         return "/cameras_head/left/image_raw"
     if camera == StretchCameras.cam_nav_rgb_se4_right:
         return "/cameras_head/right/image_raw"
-    if camera == StretchCameras.cam_nav_rgb_se4_center:
+    if camera == StretchCameras.cam_nav_rgb_se4_center or camera == StretchCameras.cam_nav_rgb_se4_center_low_rez:
         return "/camera_head/center/image_raw"
-    if camera == StretchCameras.cam_hemilidar_left:
-        return "/depth/left/depth"
-    if camera == StretchCameras.cam_hemilidar_right:
-        return "/depth/right/depth"
 
     raise NotImplementedError(f"Camera {camera} image topic mapping is not implemented")
 
@@ -1421,12 +1427,8 @@ def get_camera_info_topic_name(camera: StretchCameras):
         return "/cameras_head/left/camera_info"
     if camera == StretchCameras.cam_nav_rgb_se4_right:
         return "/cameras_head/right/camera_info"
-    if camera == StretchCameras.cam_nav_rgb_se4_center:
+    if camera == StretchCameras.cam_nav_rgb_se4_center or camera == StretchCameras.cam_nav_rgb_se4_center_low_rez:
         return "/camera_head/center/camera_info"
-    if camera == StretchCameras.cam_hemilidar_left:
-        return "/depth/left/camera_info"
-    if camera == StretchCameras.cam_hemilidar_right:
-        return "/depth/right/camera_info"
 
     raise NotImplementedError(f"Camera {camera} camera_info topic mapping is not implemented")
 
@@ -1438,10 +1440,6 @@ def get_camera_pointcloud_topic_name(camera: StretchCameras):
     """
     if camera == StretchCameras.cam_gripper_se4_stereo_depth:
         return "/gripper_camera/depth/color/points"
-    if camera == StretchCameras.cam_hemilidar_left:
-        return "/lidar_points_left"
-    if camera == StretchCameras.cam_hemilidar_right:
-        return "/lidar_points_right"
 
     raise NotImplementedError(f"Camera {camera} pointcloud topic mapping is not implemented")
 
@@ -1461,12 +1459,8 @@ def get_camera_frame(camera: StretchCameras):
         return "camera_left_optical_link"
     if camera == StretchCameras.cam_nav_rgb_se4_right:
         return "camera_right_optical_link"
-    if camera == StretchCameras.cam_nav_rgb_se4_center:
+    if camera == StretchCameras.cam_nav_rgb_se4_center or camera == StretchCameras.cam_nav_rgb_se4_center_low_rez:
         return "camera_center_optical_link"
-    if camera == StretchCameras.cam_hemilidar_left:
-        return "lidar_left_link"
-    if camera == StretchCameras.cam_hemilidar_right:
-        return "lidar_right_link"
 
     raise NotImplementedError(f"Camera {camera} frame is not implemented")