Configure the entire nav2 stack with mypy

.github/workflows/lint.yml

@@ -11,43 +11,20 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-          linter: [xmllint, cpplint, uncrustify, pep257, flake8]
+          linter: [xmllint, cpplint, uncrustify, pep257, flake8, mypy]
     steps:
     - uses: actions/checkout@v4
-    - uses: ros-tooling/action-ros-lint@v0.1
-      with:
-        linter: ${{ matrix.linter }}
-        distribution: rolling
-        package-name: "*"
 
-  ament_lint_mypy:
-    name: ament_mypy
-    runs-on: ubuntu-latest
-    container:
-      image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest
-    steps:
-    - uses: actions/checkout@v4
-
-    - name: Install typeshed
+    - name: Install typeshed for mypy
+      if: matrix.linter == 'mypy'
       run: sudo apt update && sudo apt install -y python3-typeshed
 
     - uses: ros-tooling/action-ros-lint@v0.1
       with:
-        linter: mypy
+        linter: ${{ matrix.linter }}
         distribution: rolling
-        package-name: >-
-          nav2_smac_planner
-          nav2_common
-          nav2_bringup
-          nav2_collision_monitor
-          nav2_costmap_2d
-          opennav_docking
-          nav2_lifecycle_manager
-          nav2_loopback_sim
-          nav2_map_server
-          nav2_simple_commander
-          nav2_system_tests
-        arguments: --config tools/pyproject.toml
+        package-name: "*"
+        arguments: ${{ matrix.linter == 'mypy' && '--config tools/pyproject.toml' || '' }}
 
   pre-commit:
     name: pre-commit
@@ -64,3 +41,4 @@ jobs:
             ament_xmllint,
             ament_flake8,
             ament_pep257,
+            ament_mypy

.pre-commit-config.yaml

@@ -95,3 +95,10 @@ repos:
         language: system
         types: [python]
         entry: ament_pep257
+    -   id: ament_mypy
+        name: ament_mypy
+        description: Check Python code style using mypy.
+        language: system
+        types: [python]
+        args: ["--config", "tools/pyproject.toml"]
+        entry: ament_mypy

tools/bt2img.py

@@ -17,7 +17,7 @@
 # for instructions
 
 import argparse
-import xml.etree.ElementTree
+import xml.etree.ElementTree as ET
 
 import graphviz  # pip3 install graphviz
 
@@ -75,16 +75,13 @@
     'SubTree',
 ]
 
-global xml_tree
 
-
-def main():
-    global xml_tree
+def main() -> None:
     args = parse_command_line()
-    xml_tree = xml.etree.ElementTree.parse(args.behavior_tree)
+    xml_tree = ET.parse(args.behavior_tree)
     root_tree_name = find_root_tree_name(xml_tree)
     behavior_tree = find_behavior_tree(xml_tree, root_tree_name)
-    dot = convert2dot(behavior_tree)
+    dot = convert2dot(behavior_tree, xml_tree)
     if args.legend:
         legend = make_legend()
         legend.format = 'png'
@@ -96,7 +93,7 @@ def main():
     dot.render(args.image_out, view=args.display)
 
 
-def parse_command_line():
+def parse_command_line() -> argparse.Namespace:
     parser = argparse.ArgumentParser(
         description='Convert a behavior tree XML file to an image'
     )
@@ -124,11 +121,15 @@ def parse_command_line():
     return parser.parse_args()
 
 
-def find_root_tree_name(xml_tree):
-    return xml_tree.getroot().get('main_tree_to_execute')
+def find_root_tree_name(xml_tree: ET.ElementTree) -> str:
+    root = xml_tree.getroot()
+    main_tree = root.get('main_tree_to_execute')
+    if main_tree is None:
+        raise RuntimeError('No main_tree_to_execute attribute found in XML root')
+    return main_tree
 
 
-def find_behavior_tree(xml_tree, tree_name):
+def find_behavior_tree(xml_tree: ET.ElementTree, tree_name: str) -> ET.Element:
     trees = xml_tree.findall('BehaviorTree')
     if len(trees) == 0:
         raise RuntimeError('No behavior trees were found in the XML file')
@@ -141,33 +142,50 @@ def find_behavior_tree(xml_tree, tree_name):
 
 
 # Generate a dot description of the root of the behavior tree.
-def convert2dot(behavior_tree):
+def convert2dot(behavior_tree: ET.Element, xml_tree: ET.ElementTree) -> graphviz.Digraph:
     dot = graphviz.Digraph()
     root = behavior_tree
     parent_dot_name = str(hash(root))
     dot.node(parent_dot_name, root.get('ID'), shape='box')
-    convert_subtree(dot, root, parent_dot_name)
+    convert_subtree(dot, root, parent_dot_name, xml_tree)
     return dot
 
 
 # Recursive function. We add the children to the dot file, and then recursively
 # call this function on the children. Nodes are given an ID that is the hash
 # of the node to ensure each is unique.
-def convert_subtree(dot, parent_node, parent_dot_name):
+def convert_subtree(
+    dot: graphviz.Digraph,
+    parent_node: ET.Element,
+    parent_dot_name: str,
+    xml_tree: ET.ElementTree,
+) -> None:
     if parent_node.tag == 'SubTree':
-        add_sub_tree(dot, parent_dot_name, parent_node)
+        add_sub_tree(dot, parent_dot_name, parent_node, xml_tree)
     else:
-        add_nodes(dot, parent_dot_name, parent_node)
+        add_nodes(dot, parent_dot_name, parent_node, xml_tree)
 
 
-def add_sub_tree(dot, parent_dot_name, parent_node):
+def add_sub_tree(
+    dot: graphviz.Digraph,
+    parent_dot_name: str,
+    parent_node: ET.Element,
+    xml_tree: ET.ElementTree,
+) -> None:
     root_tree_name = parent_node.get('ID')
+    if root_tree_name is None:
+        raise RuntimeError('SubTree node has no ID attribute')
     dot.node(parent_dot_name, root_tree_name, shape='box')
     behavior_tree = find_behavior_tree(xml_tree, root_tree_name)
-    convert_subtree(dot, behavior_tree, parent_dot_name)
+    convert_subtree(dot, behavior_tree, parent_dot_name, xml_tree)
 
 
-def add_nodes(dot, parent_dot_name, parent_node):
+def add_nodes(
+    dot: graphviz.Digraph,
+    parent_dot_name: str,
+    parent_node: ET.Element,
+    xml_tree: ET.ElementTree,
+) -> None:
     for node in list(parent_node):
         label = make_label(node)
         dot.node(
@@ -179,12 +197,12 @@ def add_nodes(dot, parent_dot_name, parent_node):
         )
         dot_name = str(hash(node))
         dot.edge(parent_dot_name, dot_name)
-        convert_subtree(dot, node, dot_name)
+        convert_subtree(dot, node, dot_name, xml_tree)
 
 
 # The node label contains the:
 # type, the name if provided, and the parameters.
-def make_label(node):
+def make_label(node: ET.Element) -> str:
     label = "< <table border='0' cellspacing='0' cellpadding='0'>"
     label += f"<tr><td align='text'><i>{node.tag}</i></td></tr>"
     name = node.get('name')
@@ -197,7 +215,7 @@ def make_label(node):
     return label
 
 
-def node_color(node_type):
+def node_color(node_type: str) -> str:
     if node_type in control_nodes:
         return 'chartreuse4'
     if node_type in action_nodes:
@@ -213,7 +231,7 @@ def node_color(node_type):
 
 
 # creates a legend which can be provided with the other images.
-def make_legend():
+def make_legend() -> graphviz.Digraph:
     legend = graphviz.Digraph(graph_attr={'rankdir': 'LR'})
     legend.attr(label='Legend')
     legend.node('Unknown', shape='box', style='filled', color='grey')

tools/planner_benchmarking/metrics.py

@@ -23,11 +23,15 @@
 from geometry_msgs.msg import PoseStamped
 from nav2_simple_commander.robot_navigator import BasicNavigator
 import numpy as np
+from numpy.typing import NDArray
 import rclpy
+from rclpy.time import Time
 from transforms3d.euler import euler2quat
 
 
-def getPlannerResults(navigator, initial_pose, goal_pose, planners):
+def getPlannerResults(
+        navigator: BasicNavigator, initial_pose: PoseStamped,
+        goal_pose: PoseStamped, planners: list[str]) -> list[PoseStamped]:
     results = []
     for planner in planners:
         path = navigator._getPathImpl(initial_pose, goal_pose, planner, use_start=True)
@@ -39,7 +43,9 @@ def getPlannerResults(navigator, initial_pose, goal_pose, planners):
     return results
 
 
-def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res):
+def getRandomStart(
+        costmap: NDArray[np.float32], max_cost: int,
+        side_buffer: int, time_stamp: Time, res: float) -> PoseStamped:
     start = PoseStamped()
     start.header.frame_id = 'map'
     start.header.stamp = time_stamp
@@ -61,7 +67,9 @@ def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res):
     return start
 
 
-def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res):
+def getRandomGoal(
+        costmap: NDArray[np.float32], start: PoseStamped,
+        max_cost: int, side_buffer: int, time_stamp: Time, res: float) -> PoseStamped:
     goal = PoseStamped()
     goal.header.frame_id = 'map'
     goal.header.stamp = time_stamp
@@ -91,7 +99,7 @@ def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res):
     return goal
 
 
-def main():
+def main() -> None:
     rclpy.init()
 
     navigator = BasicNavigator()
@@ -110,7 +118,7 @@ def main():
     max_cost = 210
     side_buffer = 100
     time_stamp = navigator.get_clock().now().to_msg()
-    results = []
+    results: list[PoseStamped] = []
     seed(33)
 
     random_pairs = 100

tools/planner_benchmarking/planning_benchmark_bringup.py

@@ -21,7 +21,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')
     config = os.path.join(
         get_package_share_directory('nav2_bringup'), 'params', 'nav2_params.yaml'

tools/planner_benchmarking/process_data.py

@@ -18,28 +18,31 @@
 import pickle
 
 import matplotlib.pylab as plt
+from nav2_msgs.action import ComputePathToPose
+from nav2_msgs.msg import Costmap
+from nav_msgs.msg import Path
 import numpy as np
 import seaborn as sns
 from tabulate import tabulate
 
 
-def getPaths(results):
+def getPaths(results: list[ComputePathToPose.Result]) -> list[Path]:
     paths = []
     for result in results:
         for path in result:
             paths.append(path.path)
     return paths
 
 
-def getTimes(results):
+def getTimes(results: list[ComputePathToPose.Result]) -> list[float]:
     times = []
     for result in results:
         for time in result:
             times.append(time.planning_time.nanosec / 1e09 + time.planning_time.sec)
     return times
 
 
-def getMapCoordsFromPaths(paths, resolution):
+def getMapCoordsFromPaths(paths: list[Path], resolution: float) -> list[list[float]]:
     coords = []
     for path in paths:
         x = []
@@ -52,8 +55,8 @@ def getMapCoordsFromPaths(paths, resolution):
     return coords
 
 
-def getPathLength(path):
-    path_length = 0
+def getPathLength(path: Path) -> float:
+    path_length = 0.0
     x_prev = path.poses[0].pose.position.x
     y_prev = path.poses[0].pose.position.y
     for i in range(1, len(path.poses)):
@@ -67,7 +70,7 @@ def getPathLength(path):
     return path_length
 
 
-def plotResults(costmap, paths):
+def plotResults(costmap: Costmap, paths: list[Path]) -> None:
     coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
     data = np.asarray(costmap.data)
     data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
@@ -82,12 +85,14 @@ def plotResults(costmap, paths):
     plt.show()
 
 
-def averagePathCost(paths, costmap, num_of_planners):
+def averagePathCost(
+        paths: list[Path], costmap: Costmap,
+        num_of_planners: int) -> list[list[float]]:
     coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
     data = np.asarray(costmap.data)
     data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
 
-    average_path_costs = []
+    average_path_costs: list[list[float]] = []
     for i in range(num_of_planners):
         average_path_costs.append([])
 
@@ -102,12 +107,12 @@ def averagePathCost(paths, costmap, num_of_planners):
     return average_path_costs
 
 
-def maxPathCost(paths, costmap, num_of_planners):
+def maxPathCost(paths: list[Path], costmap: Costmap, num_of_planners: int) -> list[list[float]]:
     coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution)
     data = np.asarray(costmap.data)
     data.resize(costmap.metadata.size_y, costmap.metadata.size_x)
 
-    max_path_costs = []
+    max_path_costs: list[list[float]] = []
     for i in range(num_of_planners):
         max_path_costs.append([])
 
@@ -124,7 +129,7 @@ def maxPathCost(paths, costmap, num_of_planners):
     return max_path_costs
 
 
-def main():
+def main() -> None:
 
     print('Read data')
     with open(os.getcwd() + '/results.pickle', 'rb') as f:
@@ -137,18 +142,17 @@ def main():
         costmap = pickle.load(f)
 
     paths = getPaths(results)
-    path_lengths = []
+    path_lengths_list = []
 
     for path in paths:
-        path_lengths.append(getPathLength(path))
-    path_lengths = np.asarray(path_lengths)
+        path_lengths_list.append(getPathLength(path))
+    path_lengths = np.asarray(path_lengths_list)
     total_paths = len(paths)
 
     path_lengths.resize((int(total_paths / len(planners)), len(planners)))
     path_lengths = path_lengths.transpose()
 
-    times = getTimes(results)
-    times = np.asarray(times)
+    times = np.asarray(getTimes(results))
     times.resize((int(total_paths / len(planners)), len(planners)))
     times = np.transpose(times)