Add grids to astar/combination images

autonav_ws/src/autonav_nav/src/astar.py

@@ -165,6 +165,10 @@ def create_path(self):
 
             cv2.circle(cvimg, (self.best_pos[0], self.best_pos[1]), 1, (255, 0, 0), 1)
             cvimg = cv2.resize(cvimg, (320, 320), interpolation=cv2.INTER_NEAREST)
+            # Draw a grid on the image that is the scale of the original image, so it should be a 80x80 grid scaled up 4x
+            for i in range(80):
+                cv2.line(cvimg, (0, i * 4), (320, i * 4), (85, 85, 85), 1)
+                cv2.line(cvimg, (i * 4, 0), (i * 4, 320), (85, 85, 85), 1)
             self.pathDebugImagePublisher.publish(CV_BRIDGE.cv2_to_compressed_imgmsg(cvimg))
 
     def reconstruct_path(self, path, current):

autonav_ws/src/autonav_vision/src/combination.py

@@ -91,6 +91,12 @@ def try_combine_grids(self):
                 preview_image[i, j] = 0 if combined_grid.data[i * 80 + j] <= 10 else 255
         preview_image = cv2.cvtColor(preview_image, cv2.COLOR_GRAY2RGB)
         preview_image = cv2.resize(preview_image, (320, 320))
+
+        # Draw a grid on the image that is the scale of the original image, so it should be a 80x80 grid scaled up 4x
+        for i in range(80):
+            cv2.line(preview_image, (0, i * 4), (320, i * 4), (85, 85, 85), 1)
+            cv2.line(preview_image, (i * 4, 0), (i * 4, 320), (85, 85, 85), 1)
+
         compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image)
         self.combined_grid_image_publisher.publish(compressed_image)