Mypy nav2 simple commander (#5059)

* Added most nav2_simple_commander files that can be trivially converted to be mypy compatible.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Modified edge cases and types for robot_navigator to pass mypy checks.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Added nav2_simple_commander to the linting workflow.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

* Added logs and ignored type errors due to spins w/o timeout.

Signed-off-by: Leander Stephen D'Souza <[email protected]>

---------

Signed-off-by: Leander Stephen D'Souza <[email protected]>

Author: leander-dsouza

.github/workflows/lint.yml

@@ -45,6 +45,7 @@ jobs:
           nav2_lifecycle_manager
           nav2_loopback_sim
           nav2_map_server
+          nav2_simple_commander
         arguments: --config tools/pyproject.toml
 
   pre-commit:

nav2_simple_commander/launch/assisted_teleop_example_launch.py

@@ -28,7 +28,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')

nav2_simple_commander/launch/follow_path_example_launch.py

@@ -27,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')

nav2_simple_commander/launch/inspection_demo_launch.py

@@ -27,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')

nav2_simple_commander/launch/nav_through_poses_example_launch.py

@@ -27,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')

nav2_simple_commander/launch/nav_to_pose_example_launch.py

@@ -27,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')

nav2_simple_commander/launch/picking_demo_launch.py

@@ -27,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')

nav2_simple_commander/launch/recoveries_example_launch.py

@@ -27,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')

nav2_simple_commander/launch/security_demo_launch.py

@@ -27,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')

nav2_simple_commander/launch/waypoint_follower_example_launch.py

@@ -27,7 +27,7 @@
 from launch_ros.actions import Node
 
 
-def generate_launch_description():
+def generate_launch_description() -> LaunchDescription:
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     sim_dir = get_package_share_directory('nav2_minimal_tb4_sim')
     desc_dir = get_package_share_directory('nav2_minimal_tb4_description')

nav2_simple_commander/nav2_simple_commander/costmap_2d.py

@@ -23,7 +23,12 @@
 and handling semantics found in the costmap 2d C++ API.
 """
 
+from typing import Optional
+
+from builtin_interfaces.msg import Time
+from nav_msgs.msg import OccupancyGrid
 import numpy as np
+from numpy.typing import NDArray
 
 
 class PyCostmap2D:
@@ -33,7 +38,7 @@ class PyCostmap2D:
     Costmap Python3 API for OccupancyGrids to populate from published messages
     """
 
-    def __init__(self, occupancy_map):
+    def __init__(self, occupancy_map: OccupancyGrid) -> None:
         """
         Initialize costmap2D.
 
@@ -46,49 +51,49 @@ def __init__(self, occupancy_map):
             None
 
         """
-        self.size_x = occupancy_map.info.width
-        self.size_y = occupancy_map.info.height
-        self.resolution = occupancy_map.info.resolution
-        self.origin_x = occupancy_map.info.origin.position.x
-        self.origin_y = occupancy_map.info.origin.position.y
-        self.global_frame_id = occupancy_map.header.frame_id
-        self.costmap_timestamp = occupancy_map.header.stamp
+        self.size_x: int = occupancy_map.info.width
+        self.size_y: int = occupancy_map.info.height
+        self.resolution: float = occupancy_map.info.resolution
+        self.origin_x: float = occupancy_map.info.origin.position.x
+        self.origin_y: float = occupancy_map.info.origin.position.y
+        self.global_frame_id: str = occupancy_map.header.frame_id
+        self.costmap_timestamp: Time = occupancy_map.header.stamp
         # Extract costmap
-        self.costmap = np.array(occupancy_map.data, dtype=np.uint8)
+        self.costmap: NDArray[np.uint8] = np.array(occupancy_map.data, dtype=np.uint8)
 
-    def getSizeInCellsX(self):
+    def getSizeInCellsX(self) -> int:
         """Get map width in cells."""
         return self.size_x
 
-    def getSizeInCellsY(self):
+    def getSizeInCellsY(self) -> int:
         """Get map height in cells."""
         return self.size_y
 
-    def getSizeInMetersX(self):
+    def getSizeInMetersX(self) -> float:
         """Get x axis map size in meters."""
         return (self.size_x - 1 + 0.5) * self.resolution
 
-    def getSizeInMetersY(self):
+    def getSizeInMetersY(self) -> float:
         """Get y axis map size in meters."""
         return (self.size_y - 1 + 0.5) * self.resolution
 
-    def getOriginX(self):
+    def getOriginX(self) -> float:
         """Get the origin x axis of the map [m]."""
         return self.origin_x
 
-    def getOriginY(self):
+    def getOriginY(self) -> float:
         """Get the origin y axis of the map [m]."""
         return self.origin_y
 
-    def getResolution(self):
+    def getResolution(self) -> float:
         """Get map resolution [m/cell]."""
         return self.resolution
 
-    def getGlobalFrameID(self):
+    def getGlobalFrameID(self) -> str:
         """Get global frame_id."""
         return self.global_frame_id
 
-    def getCostmapTimestamp(self):
+    def getCostmapTimestamp(self) -> Time:
         """Get costmap timestamp."""
         return self.costmap_timestamp
 
@@ -106,7 +111,7 @@ def getCostXY(self, mx: int, my: int) -> np.uint8:
             np.uint8: cost of a cell
 
         """
-        return self.costmap[self.getIndex(mx, my)]
+        return np.uint8(self.costmap[self.getIndex(mx, my)])
 
     def getCostIdx(self, index: int) -> np.uint8:
         """
@@ -121,7 +126,7 @@ def getCostIdx(self, index: int) -> np.uint8:
             np.uint8: cost of a cell
 
         """
-        return self.costmap[index]
+        return np.uint8(self.costmap[index])
 
     def setCost(self, mx: int, my: int, cost: np.uint8) -> None:
         """
@@ -160,7 +165,7 @@ def mapToWorld(self, mx: int, my: int) -> tuple[float, float]:
         wy = self.origin_y + (my + 0.5) * self.resolution
         return (wx, wy)
 
-    def worldToMapValidated(self, wx: float, wy: float):
+    def worldToMapValidated(self, wx: float, wy: float) -> tuple[Optional[int], Optional[int]]:
         """
         Get the map coordinate XY using world coordinate XY.
 

nav2_simple_commander/nav2_simple_commander/demo_inspection.py

@@ -26,7 +26,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()

nav2_simple_commander/nav2_simple_commander/demo_picking.py

@@ -41,7 +41,7 @@
 }
 
 
-def main():
+def main() -> None:
     # Received virtual request for picking item at Shelf A and bring to
     # worker at the pallet jack 7 for shipping. This request would
     # contain the shelf ID ('shelf_A') and shipping destination ('frieght_bay_3')
@@ -94,7 +94,7 @@ def main():
                 'Estimated time of arrival at '
                 + request_item_location
                 + ' for worker: '
-                + '{0:.0f}'.format(
+                + '{:.0f}'.format(
                     Duration.from_msg(feedback.estimated_time_remaining).nanoseconds
                     / 1e9
                 )

nav2_simple_commander/nav2_simple_commander/demo_recoveries.py

@@ -25,7 +25,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()

nav2_simple_commander/nav2_simple_commander/demo_security.py

@@ -27,7 +27,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -56,7 +56,7 @@ def main():
     navigator.waitUntilNav2Active()
 
     # Do security route until dead
-    while rclpy.ok():
+    while rclpy.ok():  # type: ignore[attr-defined]
         # Send our route
         route_poses = []
         pose = PoseStamped()
@@ -78,7 +78,7 @@ def main():
             if feedback and i % 5 == 0:
                 print(
                     'Estimated time to complete current route: '
-                    + '{0:.0f}'.format(
+                    + '{:.0f}'.format(
                         Duration.from_msg(feedback.estimated_time_remaining).nanoseconds
                         / 1e9
                     )

nav2_simple_commander/nav2_simple_commander/example_assisted_teleop.py

@@ -24,7 +24,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()

nav2_simple_commander/nav2_simple_commander/example_follow_path.py

@@ -22,7 +22,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -69,9 +69,9 @@ def main():
         if feedback and i % 5 == 0:
             print(
                 'Estimated distance remaining to goal position: '
-                + '{0:.3f}'.format(feedback.distance_to_goal)
+                + f'{feedback.distance_to_goal:.3f}'
                 + '\nCurrent speed of the robot: '
-                + '{0:.3f}'.format(feedback.speed)
+                + f'{feedback.speed:.3f}'
             )
 
     # Do something depending on the return code

nav2_simple_commander/nav2_simple_commander/example_nav_through_poses.py

@@ -23,7 +23,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -102,7 +102,7 @@ def main():
         if feedback and i % 5 == 0:
             print(
                 'Estimated time of arrival: '
-                + '{0:.0f}'.format(
+                + '{:.0f}'.format(
                     Duration.from_msg(feedback.estimated_time_remaining).nanoseconds
                     / 1e9
                 )

nav2_simple_commander/nav2_simple_commander/example_nav_to_pose.py

@@ -23,7 +23,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -82,7 +82,7 @@ def main():
         if feedback and i % 5 == 0:
             print(
                 'Estimated time of arrival: '
-                + '{0:.0f}'.format(
+                + '{:.0f}'.format(
                     Duration.from_msg(feedback.estimated_time_remaining).nanoseconds
                     / 1e9
                 )

nav2_simple_commander/nav2_simple_commander/example_waypoint_follower.py

@@ -23,7 +23,7 @@
 """
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()