Skip to content

Commit

Permalink
fix majority of python linting errors introduced in python costmap AP…
Browse files Browse the repository at this point in the history
…I additions to get CI turning over again (#3223)

* fix majority of python linting errors

* finish linting
  • Loading branch information
SteveMacenski authored Sep 23, 2022
1 parent 80909a1 commit af72761
Show file tree
Hide file tree
Showing 2 changed files with 64 additions and 40 deletions.
103 changes: 63 additions & 40 deletions nav2_simple_commander/nav2_simple_commander/costmap_2d.py
Original file line number Diff line number Diff line change
Expand Up @@ -34,13 +34,12 @@ class PyCostmap2D:

def __init__(self, occupancy_map):
"""
Initializer for costmap2D.
Initialize costmap2D.
Args:
----
occupancy_map (OccupancyGrid): 2D OccupancyGrid Map
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
Expand Down Expand Up @@ -92,49 +91,65 @@ 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
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
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
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
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
Expand All @@ -144,13 +159,17 @@ 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
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)
Expand All @@ -160,10 +179,14 @@ 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
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
Original file line number Diff line number Diff line change
Expand Up @@ -299,6 +299,7 @@ def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
def _getPathImpl(self, start, goal, planner_id='', use_start=False):
"""
Send a `ComputePathToPose` action request.
Internal implementation to get the full result, not just the path.
"""
self.debug("Waiting for 'ComputePathToPose' action server")
Expand Down

0 comments on commit af72761

Please sign in to comment.