Add mypy for static analysis

Author: Flova

Diff for: bitbots_behavior/bitbots_blackboard/CMakeLists.txt

@@ -6,17 +6,17 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
-find_package(bio_ik_msgs REQUIRED)
 find_package(ament_cmake REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(rclpy REQUIRED)
-find_package(tf2 REQUIRED)
+find_package(bio_ik_msgs REQUIRED)
+find_package(bitbots_docs REQUIRED)
 find_package(bitbots_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(bitbots_docs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 
 set(INCLUDE_DIRS
     ${bio_ik_msgs_INCLUDE_DIRS}
@@ -76,6 +76,11 @@ ament_export_dependencies(geometry_msgs)
 ament_export_dependencies(bitbots_docs)
 ament_export_include_directories(${INCLUDE_DIRS})
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_mypy REQUIRED)
+  ament_mypy(CONFIG_FILE "${CMAKE_CURRENT_LIST_DIR}/mypy.ini")
+endif()
+
 ament_python_install_package(${PROJECT_NAME})
 
 ament_package()

Diff for: bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/__init__.py

@@ -1,4 +1,4 @@
-from typing import TYPE_CHECKING, Optional
+from typing import TYPE_CHECKING
 
 from bitbots_utils.utils import nobeartype
 from rclpy.node import Node
@@ -11,6 +11,6 @@ class AbstractBlackboardCapsule:
     """Abstract class for blackboard capsules."""
 
     @nobeartype
-    def __init__(self, node: Node, blackboard: Optional["BodyBlackboard"] = None):
+    def __init__(self, node: Node, blackboard: "BodyBlackboard"):
         self._node = node
         self._blackboard = blackboard

Diff for: bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py

@@ -81,6 +81,7 @@ def publish_costmap(self) -> None:
         """
         Publishes the costmap for rviz
         """
+        assert self.costmap is not None, "Costmap is not initialized"
         # Normalize costmap to match the rviz color scheme in a good way
         normalized_costmap = (
             (255 - ((self.costmap - np.min(self.costmap)) / (np.max(self.costmap) - np.min(self.costmap))) * 255 / 2.1)
@@ -157,6 +158,7 @@ def calc_gradients(self) -> None:
         """
         Recalculates the gradient map based on the current costmap.
         """
+        assert self.base_costmap is not None, "Base costmap is not initialized"
         gradient = np.gradient(self.base_costmap)
         norms = np.linalg.norm(gradient, axis=0)
 
@@ -203,8 +205,8 @@ def calc_base_costmap(self) -> None:
 
         # Create Grid
         grid_x, grid_y = np.mgrid[
-            0 : self.field_length + self.map_margin * 2 : (self.field_length + self.map_margin * 2) * 10j,
-            0 : self.field_width + self.map_margin * 2 : (self.field_width + self.map_margin * 2) * 10j,
+            0 : self.field_length + self.map_margin * 2 : (self.field_length + self.map_margin * 2) * 10j,  # type: ignore[misc]
+            0 : self.field_width + self.map_margin * 2 : (self.field_width + self.map_margin * 2) * 10j,  # type: ignore[misc]
         ]
 
         fix_points: List[Tuple[Tuple[float, float], float]] = []
@@ -278,7 +280,8 @@ def calc_base_costmap(self) -> None:
         )
 
         # Smooth the costmap to get more continuous gradients
-        self.base_costmap = gaussian_filter(interpolated, self.body_config["base_costmap_smoothing_sigma"])
+        base_costmap: np.ndarray = gaussian_filter(interpolated, self.body_config["base_costmap_smoothing_sigma"])
+        self.base_costmap = base_costmap
         self.costmap = self.base_costmap.copy()
 
     def get_gradient_at_field_position(self, x: float, y: float) -> Tuple[float, float]:
@@ -287,6 +290,7 @@ def get_gradient_at_field_position(self, x: float, y: float) -> Tuple[float, flo
         :param x: Field coordinate in the x direction
         :param y: Field coordinate in the y direction
         """
+        assert self.gradient_map is not None, "Gradient map is not initialized"
         idx_x, idx_y = self.field_2_costmap_coord(x, y)
         return -self.gradient_map[0][idx_x, idx_y], -self.gradient_map[1][idx_x, idx_y]
 
@@ -296,6 +300,7 @@ def get_cost_at_field_position(self, x: float, y: float) -> float:
         :param x: Field coordinate in the x direction
         :param y: Field coordinate in the y direction
         """
+        assert self.costmap is not None, "Costmap is not initialized"
         idx_x, idx_y = self.field_2_costmap_coord(x, y)
         return self.costmap[idx_x, idx_y]
 
@@ -358,6 +363,7 @@ def get_cost_of_kick(self, x: float, y: float, direction: float, kick_length: fl
         :param kick_length: The length of the kick
         :param angular_range: The angular range of the kick
         """
+        assert self.costmap is not None, "Costmap is not initialized"
 
         # create a mask in the size of the costmap consisting of 8-bit values initialized as 0
         mask = Image.new("L", (self.costmap.shape[1], self.costmap.shape[0]))

Diff for: bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py

@@ -15,8 +15,8 @@ def __init__(self, node, blackboard=None):
         self.gamestate = GameState()
         self.last_update: float = 0.0
         self.unpenalized_time: float = 0.0
-        self.last_goal_from_us_time = -86400
-        self.last_goal_time = -86400
+        self.last_goal_from_us_time = -86400.0
+        self.last_goal_time = -86400.0
         self.free_kick_kickoff_team: Optional[bool] = None
 
     def get_gamestate(self) -> int:

Diff for: bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py

@@ -23,7 +23,7 @@ class KickCapsule(AbstractBlackboardCapsule):
     is_currently_kicking: bool = False
 
     __connected: bool = False
-    __action_client: ActionClient = None
+    __action_client: Optional[ActionClient] = None
 
     class WalkKickTargets(Flag):
         """

Diff for: bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py

@@ -7,7 +7,7 @@
 from bitbots_blackboard.capsules import AbstractBlackboardCapsule
 from bitbots_msgs.msg import Audio, HeadMode, RobotControlState
 
-THeadMode: TypeAlias = Literal[
+THeadMode: TypeAlias = Literal[  # type: ignore[valid-type]
     HeadMode.BALL_MODE,
     HeadMode.FIELD_FEATURES,
     HeadMode.LOOK_FORWARD,

Diff for: bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py

@@ -1,4 +1,4 @@
-from typing import Dict, List, Literal, Optional, Tuple
+from typing import List, Literal, Optional, Tuple
 
 import numpy as np
 from bitbots_utils.utils import get_parameters_from_other_node
@@ -27,7 +27,7 @@ def __init__(self, node, blackboard):
 
         # Data
         # indexed with one to match robot ids
-        self.team_data: Dict[int, TeamData] = {}
+        self.team_data: dict[int, TeamData] = {}
         for i in range(1, 7):
             self.team_data[i] = TeamData()
         self.times_to_ball = dict()
@@ -158,7 +158,8 @@ def get_role(self) -> Tuple[int, float]:
         return self.strategy.role, self.role_update
 
     def set_kickoff_strategy(
-        self, strategy: Literal[Strategy.SIDE_LEFT, Strategy.SIDE_MIDDLE, Strategy.SIDE_RIGHT]
+        self,
+        strategy: Literal[Strategy.SIDE_LEFT, Strategy.SIDE_MIDDLE, Strategy.SIDE_RIGHT],  # type: ignore[valid-type]
     ) -> None:
         self.strategy.offensive_side = strategy
         self.strategy_update = self._node.get_clock().now().nanoseconds / 1e9
@@ -184,7 +185,7 @@ def is_not_goalie(team_data: TeamData) -> bool:
 
         # Remove goalie data if needed
         if not count_goalie:
-            team_data_infos = filter(is_not_goalie, team_data_infos)
+            team_data_infos = filter(is_not_goalie, team_data_infos)  # type: ignore[assignment]
 
         # Count valid team data infos (aka robots with valid team data)
         return sum(map(self.is_valid, team_data_infos))
@@ -194,10 +195,10 @@ def is_a_goalie(team_data: TeamData) -> bool:
             return team_data.strategy.role == Strategy.ROLE_GOALIE
 
         # Get the team data infos for all robots (ignoring the robot id/name)
-        team_data_infos = self.team_data.values()
+        team_data_infos = self.team_data.values()  # type: ignore[assignment]
 
         # Remove none goalie Data
-        team_data_infos = filter(is_a_goalie, team_data_infos)
+        team_data_infos = filter(is_a_goalie, team_data_infos)  # type: ignore[assignment]
 
         # Count valid team data infos (aka robots with valid team data)
         return sum(map(self.is_valid, team_data_infos)) == 1
@@ -246,3 +247,4 @@ def get_ball_max_covariance(team_data: TeamData) -> float:
             return PointStamped(
                 header=team_data_with_best_ball.header, point=team_data_with_best_ball.ball_absolute.pose.position
             )
+        return None

Diff for: bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py

@@ -1,5 +1,5 @@
 import math
-from typing import Optional, Tuple
+from typing import Tuple
 
 import numpy as np
 import tf2_ros as tf2
@@ -22,6 +22,18 @@
 from bitbots_blackboard.capsules import AbstractBlackboardCapsule
 
 
+class WorldModelTFError(Exception):
+    ...
+
+
+class WorldModelPositionTFError(WorldModelTFError):
+    ...
+
+
+class WorldModelBallTFError(WorldModelTFError):
+    ...
+
+
 class WorldModelCapsule(AbstractBlackboardCapsule):
     """Provides information about the world model."""
 
@@ -118,7 +130,7 @@ def get_ball_position_uv(self) -> Tuple[float, float]:
         except tf2.ExtrapolationException as e:
             self._node.get_logger().warn(str(e))
             self._node.get_logger().error("Severe transformation problem concerning the ball!")
-            return None
+            raise WorldModelBallTFError("Could not transform ball to base_footprint frame") from e
         return ball_bfp.x, ball_bfp.y
 
     def get_ball_distance(self) -> float:
@@ -210,23 +222,21 @@ def get_map_based_opp_goal_right_post_uv(self) -> Tuple[float, float]:
     # Pose #
     ########
 
-    def get_current_position(self) -> Optional[Tuple[float, float, float]]:
+    def get_current_position(self) -> Tuple[float, float, float]:
         """
         Returns the current position on the field as determined by the localization.
         0,0,0 is the center of the field looking in the direction of the opponent goal.
         :returns x,y,theta:
         """
-        if not (transform := self.get_current_position_transform()):
-            return None
+        transform = self.get_current_position_transform()
         theta = euler_from_quaternion(numpify(transform.transform.rotation))[2]
         return transform.transform.translation.x, transform.transform.translation.y, theta
 
-    def get_current_position_pose_stamped(self) -> Optional[PoseStamped]:
+    def get_current_position_pose_stamped(self) -> PoseStamped:
         """
         Returns the current position as determined by the localization as a PoseStamped
         """
-        if not (transform := self.get_current_position_transform()):
-            return None
+        transform = self.get_current_position_transform()
         return PoseStamped(
             header=transform.header,
             pose=Pose(
@@ -245,7 +255,7 @@ def get_current_position_transform(self) -> TransformStamped:
             )
         except (tf2.LookupException, tf2.ConnectivityException, tf2.ExtrapolationException) as e:
             self._node.get_logger().warn(str(e))
-            return None
+            raise WorldModelPositionTFError("Could not get current position transform") from e
 
     ##########
     # Common #

Diff for: bitbots_behavior/bitbots_blackboard/mypy.ini

@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True

Diff for: bitbots_behavior/bitbots_blackboard/package.xml

@@ -37,6 +37,10 @@
     <exec_depend>std_srvs</exec_depend>
     <exec_depend>tf2_geometry_msgs</exec_depend>
     <exec_depend>tf2</exec_depend>
+    <test_depend>ament_mypy</test_depend>
+    <test_depend>ament_cmake_mypy</test_depend>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
 
     <export>
         <bitbots_documentation>

Diff for: bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to.py

@@ -12,7 +12,7 @@
 class GoToRelativePosition(AbstractActionElement):
     blackboard: BodyBlackboard
 
-    def __init__(self, blackboard, dsd, parameters: dict = None):
+    def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
         self.point = float(parameters.get("x", 0)), float(parameters.get("y", 0)), float(parameters.get("t", 0))
         self.threshold = float(parameters.get("threshold", 0.1))

Diff for: bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/aligned_to_goal.py

@@ -26,10 +26,6 @@ def perform(self, reevaluate=False):
         # Get our position
         current_pose = self.blackboard.world_model.get_current_position()
 
-        # Check if we know our position
-        if current_pose is None:
-            return "NO"
-
         # Get maximum kick angle relative to our base footprint
         angle_difference_right = current_pose[2] - self.max_kick_angle
         angle_difference_left = current_pose[2] + self.max_kick_angle

Diff for: bitbots_behavior/bitbots_body_behavior/package.xml

@@ -30,6 +30,7 @@
   <depend>soccer_vision_3d_msgs</depend>
   <depend>tf_transformations</depend>
   <depend>tf2</depend>
+  <test_depend>ament_mypy</test_depend>
 
 
   <export>

Diff for: bitbots_behavior/bitbots_body_behavior/test/mypy.ini

@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True

Diff for: bitbots_behavior/bitbots_body_behavior/test/test_mypy.py

@@ -0,0 +1,11 @@
+from pathlib import Path
+
+from ament_mypy.main import main
+
+import pytest
+
+
+@pytest.mark.mypy
+def test_mypy():
+    rc = main(argv=["--config", str((Path(__file__).parent / "mypy.ini").resolve())])
+    assert rc == 0, 'Found code style errors / warnings'