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.
@@ -14,6 +15,13 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+"""
+This is a Python3 API for costmap 2d messages from the stack.
+
+It provides the basic conversion, get/set,
+and handling semantics found in the costmap 2d C++ API.
+"""
+
 import numpy as np
 
 
@@ -25,6 +33,15 @@ class PyCostmap2D:
     """
 
     def __init__(self, occupancy_map):
+        """
+        Initializer for costmap2D.
+
+        Initialize instance variables with parameter occupancy_map.
+            Args:
+                occupancy_map (OccupancyGrid): 2D OccupancyGrid Map
+            Returns:
+                None
+        """
         self.size_x = occupancy_map.info.width
         self.size_y = occupancy_map.info.height
         self.resolution = occupancy_map.info.resolution
@@ -33,8 +50,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.uint8)
 
     def getSizeInCellsX(self):
         """Get map width in cells."""
@@ -46,11 +62,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 +87,83 @@ def getGlobalFrameID(self):
     def getCostmapTimestamp(self):
         """Get costmap timestamp."""
         return self.costmap_timestamp
+
+    def getCostXY(self, mx: int, my: int) -> np.uint8:
+        """
+        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.uint8: cost of a cell
+        """
+        return self.costmap[self.getIndex(mx, my)]
+
+    def getCostIdx(self, index: int) -> np.uint8:
+        """
+        Get the cost of a cell in the costmap using Index.
+
+            Args:
+                index (int): index of cell to get cost
+            Returns:
+                np.uint8: cost of a cell
+        """
+        return self.costmap[index]
+
+    def setCost(self, mx: int, my: int, cost: np.uint8) -> 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 (np.uint8): The cost to set the cell
+            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 of float: wx, wy
+                wx (float) [m]: world coordinate X
+                wy (float) [m]: 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 (float) [m]: world coordinate X to get map coordinate
+                wy (float) [m]: world coordinate Y to get map coordinate
+            Returns:
+                tuple of 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