Merge branch 'main' into feature/throw_in

Author: confusedlama

.github/workflows/pre-commit.yml

@@ -7,7 +7,7 @@ on:
 
 jobs:
   pre-commit:
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v4
       - uses: actions/setup-python@v4

.vscode/settings.json

@@ -222,9 +222,11 @@
     "ros.distro": "iron",
     "search.useIgnoreFiles": false,
     "python.autoComplete.extraPaths": [
+        "/opt/openrobots/lib/python3.10/site-packages",
         "/opt/ros/iron/lib/python3.10/site-packages"
     ],
     "python.analysis.extraPaths": [
+        "/opt/openrobots/lib/python3.10/site-packages",
         "/opt/ros/iron/lib/python3.10/site-packages"
     ],
     "cmake.configureOnOpen": false,

bitbots_behavior/bitbots_blackboard/CMakeLists.txt

@@ -1,80 +1,16 @@
 cmake_minimum_required(VERSION 3.5)
 project(bitbots_blackboard)
 
-# Add support for C++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(bitbots_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(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 
-set(INCLUDE_DIRS
-    ${bio_ik_msgs_INCLUDE_DIRS}
-    ${ament_cmake_INCLUDE_DIRS}
-    ${sensor_msgs_INCLUDE_DIRS}
-    ${rclpy_INCLUDE_DIRS}
-    ${tf2_INCLUDE_DIRS}
-    ${bitbots_msgs_INCLUDE_DIRS}
-    ${std_msgs_INCLUDE_DIRS}
-    ${tf2_geometry_msgs_INCLUDE_DIRS}
-    ${std_srvs_INCLUDE_DIRS}
-    ${geometry_msgs_INCLUDE_DIRS}
-    ${bitbots_docs_INCLUDE_DIRS})
-include_directories(${INCLUDE_DIRS})
-
-set(LIBRARY_DIRS
-    ${bio_ik_msgs_LIBRARY_DIRS}
-    ${ament_cmake_LIBRARY_DIRS}
-    ${sensor_msgs_LIBRARY_DIRS}
-    ${rclpy_LIBRARY_DIRS}
-    ${tf2_LIBRARY_DIRS}
-    ${bitbots_msgs_LIBRARY_DIRS}
-    ${std_msgs_LIBRARY_DIRS}
-    ${tf2_geometry_msgs_LIBRARY_DIRS}
-    ${std_srvs_LIBRARY_DIRS}
-    ${geometry_msgs_LIBRARY_DIRS}
-    ${bitbots_docs_LIBRARY_DIRS})
-
-link_directories(${LIBRARY_DIRS})
-
-set(LIBS
-    ${bio_ik_msgs_LIBRARIES}
-    ${ament_cmake_LIBRARIES}
-    ${sensor_msgs_LIBRARIES}
-    ${rclpy_LIBRARIES}
-    ${tf2_LIBRARIES}
-    ${bitbots_msgs_LIBRARIES}
-    ${std_msgs_LIBRARIES}
-    ${tf2_geometry_msgs_LIBRARIES}
-    ${std_srvs_LIBRARIES}
-    ${geometry_msgs_LIBRARIES}
-    ${bitbots_docs_LIBRARIES})
-
-include(${CMAKE_BINARY_DIR}/../bitbots_docs/enable_bitbots_docs.cmake)
 enable_bitbots_docs()
 
-ament_export_dependencies(bio_ik_msgs)
-ament_export_dependencies(ament_cmake)
-ament_export_dependencies(sensor_msgs)
-ament_export_dependencies(rclpy)
-ament_export_dependencies(tf2)
-ament_export_dependencies(bitbots_msgs)
-ament_export_dependencies(std_msgs)
-ament_export_dependencies(tf2_geometry_msgs)
-ament_export_dependencies(std_srvs)
-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})
 

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/__init__.py

@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/body_blackboard.py

@@ -13,7 +13,7 @@
 
 
 class BodyBlackboard:
-    def __init__(self, node: Node, tf_buffer: tf2.Buffer):
+    def __init__(self, node: Node, tf_buffer: tf2.BufferInterface):
         # References
         self.node = node
         self.tf_buffer = tf_buffer

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/__init__.py

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

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py

@@ -1,5 +1,5 @@
 import math
-from typing import List, Optional, Tuple
+from typing import Optional
 
 import numpy as np
 import tf2_ros as tf2
@@ -53,7 +53,7 @@ def __init__(self, node, blackboard):
         self.calc_base_costmap()
         self.calc_gradients()
 
-    def robot_callback(self, msg: RobotArray):
+    def robot_callback(self, msg: RobotArray) -> None:
         """
         Callback with new robot detections
         """
@@ -77,10 +77,11 @@ def robot_callback(self, msg: RobotArray):
         # Publish debug costmap
         self.publish_costmap()
 
-    def publish_costmap(self):
+    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)
@@ -131,7 +132,7 @@ def get_pass_regions(self) -> np.ndarray:
         # Smooth obstacle map
         return gaussian_filter(costmap, pass_smooth)
 
-    def field_2_costmap_coord(self, x: float, y: float) -> Tuple[float, float]:
+    def field_2_costmap_coord(self, x: float, y: float) -> tuple[int, int]:
         """
         Converts a field position to the corresponding indices for the costmap.
 
@@ -153,10 +154,11 @@ def field_2_costmap_coord(self, x: float, y: float) -> Tuple[float, float]:
         )
         return idx_x, idx_y
 
-    def calc_gradients(self):
+    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)
 
@@ -186,7 +188,7 @@ def cost_at_relative_xy(self, x: float, y: float) -> float:
 
         return self.get_cost_at_field_position(point.point.x, point.point.y)
 
-    def calc_base_costmap(self):
+    def calc_base_costmap(self) -> None:
         """
         Builds the base costmap based on the behavior parameters.
         This costmap includes a gradient towards the enemy goal and high costs outside the playable area
@@ -203,11 +205,11 @@ def calc_base_costmap(self):
 
         # 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]] = []
+        fix_points: list[tuple[tuple[float, float], float]] = []
 
         # Add base points
         fix_points.extend(
@@ -278,15 +280,17 @@ def calc_base_costmap(self):
         )
 
         # 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]:
+    def get_gradient_at_field_position(self, x: float, y: float) -> tuple[float, float]:
         """
         Gets the gradient tuple at a given field position
         :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,10 +300,11 @@ 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]
 
-    def get_gradient_direction_at_field_position(self, x: float, y: float):
+    def get_gradient_direction_at_field_position(self, x: float, y: float) -> float:
         """
         Returns the gradient direction at the given position
         :param x: Field coordinate in the x direction
@@ -318,7 +323,9 @@ def get_gradient_direction_at_field_position(self, x: float, y: float):
         grad = self.get_gradient_at_field_position(x, y)
         return math.atan2(grad[1], grad[0])
 
-    def get_cost_of_kick_relative(self, x: float, y: float, direction: float, kick_length: float, angular_range: float):
+    def get_cost_of_kick_relative(
+        self, x: float, y: float, direction: float, kick_length: float, angular_range: float
+    ) -> float:
         """
         Returns the cost of a kick at the given position and direction in base footprint frame
         :param x: Field coordinate in the x direction
@@ -356,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]))
@@ -386,7 +394,7 @@ def get_cost_of_kick(self, x: float, y: float, direction: float, kick_length: fl
         # This should contribute way less than the max and should have an impact if the max values are similar in all directions.
         return masked_costmap.max() * 0.75 + masked_costmap.min() * 0.25
 
-    def get_current_cost_of_kick(self, direction: float, kick_length: float, angular_range: float):
+    def get_current_cost_of_kick(self, direction: float, kick_length: float, angular_range: float) -> float:
         """
         Returns the cost of the kick at the current position
         :param direction: The direction of the kick