simple command costmap api - update few functions (#3169)

* * add aditional function to costmap_2d.py

Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>

Updated-by: Jaehun Kim <k9632441@gmail.com>

* finish task B

* Update nav2_simple_commander/nav2_simple_commander/costmap_2d.py

* Update method docs

* Remove underscores at parameters and split getCost into getCostXY and getCostIdx

* Update method docstrings

* lint code & update docstring, remove default value of getCostXY

* lint code with pep257 & flake8

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