simple command costmap api - update few functions

nav2_simple_commander/nav2_simple_commander/costmap_2d.py

@@ -1,6 +1,7 @@
 #! /usr/bin/env python3
 # Copyright 2021 Samsung Research America
 # Copyright 2022 Stevedan Ogochukwu Omodolor
+# Copyright 2022 Jaehun Jackson Kim
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -33,8 +34,7 @@ def __init__(self, occupancy_map):
         self.global_frame_id = occupancy_map.header.frame_id
         self.costmap_timestamp = occupancy_map.header.stamp
         # Extract costmap
-        self.costmap = np.array(occupancy_map.data, dtype=np.int8).reshape(
-            self.size_y, self.size_x)
+        self.costmap = np.array(occupancy_map.data, dtype=np.int8)
 
     def getSizeInCellsX(self):
         """Get map width in cells."""
@@ -46,11 +46,11 @@ def getSizeInCellsY(self):
 
     def getSizeInMetersX(self):
         """Get x axis map size in meters."""
-        return (self.size_x - 1 + 0.5) * self.resolution_
+        return (self.size_x - 1 + 0.5) * self.resolution
 
     def getSizeInMetersY(self):
         """Get y axis map size in meters."""
-        return (self.size_y - 1 + 0.5) * self.resolution_
+        return (self.size_y - 1 + 0.5) * self.resolution
 
     def getOriginX(self):
         """Get the origin x axis of the map [m]."""
@@ -71,3 +71,78 @@ def getGlobalFrameID(self):
     def getCostmapTimestamp(self):
         """Get costmap timestamp."""
         return self.costmap_timestamp
+
+    def getCostXY(self, mx: int = None, my: int = None) -> np.int8:
+        """
+        Get the cost of a cell in the costmap using map coordinate XY
+            Args:
+                mx (int): map coordinate X to get cost
+                my (int): map coordinate Y to get cost
+            Returns:
+                np.int8: cost of a cell
+        """
+        return self.costmap[self.getIndex(mx, my)]
+
+    def getCostIdx(self, index: int) -> np.int8:
+        """
+        Get the cost of a cell in the costmap using Index
+            Args:
+                index (int): index of cell to get cost
+            Returns:
+                np.int8: cost of a cell
+        """
+        return self.costmap[index]
+
+    def setCost(self, mx: int, my: int, cost: np.int8) -> None:
+        """
+        Set the cost of a cell in the costmap using map coordinate XY
+            Args:
+                mx (int): map coordinate X to get cost
+                my (int): map coordinate Y to get cost
+                cost (int): The cost to set the cell to
+            Returns:
+                None
+        """
+        self.costmap[self.getIndex(mx, my)] = cost
+
+    def mapToWorld(self, mx: int, my: int) -> tuple[float, float]:
+        """
+        Get the world coordinate XY using map coordinate XY
+            Args:
+                mx (int): map coordinate X to get world coordinate
+                my (int): map coordinate Y to get world coordinate
+            Returns:
+                tuple[float, float]: wx, wy
+                wx (float): world coordinate X
+                wy (float): world coordinate Y
+        """
+        wx = self.origin_x + (mx + 0.5) * self.resolution
+        wy = self.origin_y + (my + 0.5) * self.resolution
+        return (wx, wy)
+
+    def worldToMap(self, wx: float, wy: float) -> tuple[int, int]:
+        """
+        Get the map coordinate XY using world coordinate XY
+            Args:
+                wx (int): world coordinate X to get map coordinate
+                wy (int): world coordinate Y to get map coordinate
+            Returns:
+                tuple[int, int]: mx, my
+                mx (int): map coordinate X
+                my (int): map coordinate Y
+        """
+        mx = int((wx - self.origin_x) // self.resolution)
+        my = int((wy - self.origin_y) // self.resolution)
+        return (mx, my)
+
+    def getIndex(self, mx: int, my: int) -> int:
+        """
+        Get the index of the cell using map coordinate XY
+            Args:
+                mx (int): map coordinate X to get Index
+                my (int): map coordinate Y to get Index
+            Returns:
+                int: The index of the cell
+        """
+        return my * self.size_x + mx
+