diff --git a/.vscode/settings.json b/.vscode/settings.json
index be0dde9f6..7168aec42 100644
--- a/.vscode/settings.json
+++ b/.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,
diff --git a/bitbots_behavior/bitbots_blackboard/CMakeLists.txt b/bitbots_behavior/bitbots_blackboard/CMakeLists.txt
index be7e5dad5..c363d76a8 100644
--- a/bitbots_behavior/bitbots_blackboard/CMakeLists.txt
+++ b/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 --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/__init__.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/__init__.py
+++ b/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()
diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/body_blackboard.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/body_blackboard.py
index 813f86efe..436f060f9 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/body_blackboard.py
+++ b/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
diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/__init__.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/__init__.py
index 20c418e7c..62d4cd368 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/__init__.py
+++ b/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
diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py
index bf51664f6..466381dc0 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py
+++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py
@@ -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,8 +205,8 @@ 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]] = []
@@ -278,7 +280,8 @@ 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]:
@@ -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,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
diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py
index 58cbcab5a..0f52543a6 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py
+++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py
@@ -1,3 +1,5 @@
+from typing import Optional
+
from bitbots_utils.utils import get_parameters_from_other_node
from game_controller_hl_interfaces.msg import GameState
@@ -9,73 +11,63 @@ class GameStatusCapsule(AbstractBlackboardCapsule):
def __init__(self, node, blackboard=None):
super().__init__(node, blackboard)
- self.team_id = get_parameters_from_other_node(self._node, "parameter_blackboard", ["team_id"])["team_id"]
+ self.team_id: int = get_parameters_from_other_node(self._node, "parameter_blackboard", ["team_id"])["team_id"]
self.gamestate = GameState()
- self.last_update = 0
- self.unpenalized_time = 0
- self.last_goal_from_us_time = -86400
- self.last_goal_time = -86400
- self.free_kick_kickoff_team = None
-
- def is_game_state_equals(self, value):
- assert value in [
- GameState.GAMESTATE_PLAYING,
- GameState.GAMESTATE_FINISHED,
- GameState.GAMESTATE_INITIAL,
- GameState.GAMESTATE_READY,
- GameState.GAMESTATE_SET,
- ]
- return value == self.get_gamestate()
-
- def get_gamestate(self):
+ self.last_update: float = 0.0
+ self.unpenalized_time: float = 0.0
+ 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:
return self.gamestate.game_state
- def get_secondary_state(self):
+ def get_secondary_state(self) -> int:
return self.gamestate.secondary_state
- def get_secondary_state_mode(self):
+ def get_secondary_state_mode(self) -> int:
return self.gamestate.secondary_state_mode
- def get_secondary_team(self):
+ def get_secondary_team(self) -> int:
return self.gamestate.secondary_state_team
- def has_kickoff(self):
+ def has_kickoff(self) -> bool:
return self.gamestate.has_kick_off
- def has_penalty_kick(self):
+ def has_penalty_kick(self) -> bool:
return (
self.gamestate.secondary_state == GameState.STATE_PENALTYKICK
or self.gamestate.secondary_state == GameState.STATE_PENALTYSHOOT
) and self.gamestate._secondary_state_team == self.team_id
- def get_own_goals(self):
+ def get_our_goals(self) -> int:
return self.gamestate.own_score
- def get_opp_goals(self):
+ def get_opp_goals(self) -> int:
return self.gamestate.rival_score
- def get_seconds_since_own_goal(self):
+ def get_seconds_since_own_goal(self) -> float:
return self._node.get_clock().now().nanoseconds / 1e9 - self.last_goal_from_us_time
- def get_seconds_since_any_goal(self):
+ def get_seconds_since_any_goal(self) -> float:
return self._node.get_clock().now().nanoseconds / 1e9 - self.last_goal_time
- def get_seconds_remaining(self):
+ def get_seconds_remaining(self) -> float:
# Time from the message minus time passed since receiving it
return max(
- self.gamestate.seconds_remaining - (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update), 0
+ self.gamestate.seconds_remaining - (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update), 0.0
)
- def get_secondary_seconds_remaining(self):
+ def get_secondary_seconds_remaining(self) -> float:
"""Seconds remaining for things like kickoff"""
# Time from the message minus time passed since receiving it
return max(
self.gamestate.secondary_seconds_remaining
- (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update),
- 0,
+ 0.0,
)
- def get_seconds_since_last_drop_ball(self):
+ def get_seconds_since_last_drop_ball(self) -> Optional[float]:
"""Returns the seconds since the last drop in"""
if self.gamestate.drop_in_time == -1:
return None
@@ -83,22 +75,22 @@ def get_seconds_since_last_drop_ball(self):
# Time from the message plus seconds passed since receiving it
return self.gamestate.drop_in_time + (self._node.get_clock().now().nanoseconds / 1e9 - self.last_update)
- def get_seconds_since_unpenalized(self):
+ def get_seconds_since_unpenalized(self) -> float:
return self._node.get_clock().now().nanoseconds / 1e9 - self.unpenalized_time
- def get_is_penalized(self):
+ def get_is_penalized(self) -> bool:
return self.gamestate.penalized
- def received_gamestate(self):
- return self.last_update != 0
+ def received_gamestate(self) -> bool:
+ return self.last_update != 0.0
- def get_team_id(self):
+ def get_team_id(self) -> int:
return self.team_id
- def get_red_cards(self):
+ def get_red_cards(self) -> int:
return self.gamestate.team_mates_with_red_card
- def gamestate_callback(self, gamestate_msg: GameState):
+ def gamestate_callback(self, gamestate_msg: GameState) -> None:
if self.gamestate.penalized and not gamestate_msg.penalized:
self.unpenalized_time = self._node.get_clock().now().nanoseconds / 1e9
diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py
index 6d9eaf628..6ed79c92b 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py
+++ b/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):
"""
@@ -43,14 +43,14 @@ def __init__(self, node, blackboard):
self.walk_kick_pub = self._node.create_publisher(Bool, "/kick", 1)
# self.connect_dynamic_kick() Do not connect if dynamic_kick is disabled
- def walk_kick(self, target: WalkKickTargets):
+ def walk_kick(self, target: WalkKickTargets) -> None:
"""
Kick the ball while walking
:param target: Target for the walk kick (e.g. left or right foot)
"""
self.walk_kick_pub.publish(Bool(data=target.value))
- def connect_dynamic_kick(self):
+ def connect_dynamic_kick(self) -> None:
topic = self._blackboard.config["dynamic_kick"]["topic"]
self.__action_client = ActionClient(self._node, Kick, topic, callback_group=ReentrantCallbackGroup())
self.__connected = self.__action_client.wait_for_server(self._blackboard.config["dynamic_kick"]["wait_time"])
@@ -58,7 +58,7 @@ def connect_dynamic_kick(self):
if not self.__connected:
self._node.get_logger().error(f"No dynamic_kick server running on {topic}")
- def dynamic_kick(self, goal: Kick.Goal):
+ def dynamic_kick(self, goal: Kick.Goal) -> None:
"""
:param goal: Goal to kick to
:type goal: KickGoal
diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py
index 490eee491..90107d664 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py
+++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py
@@ -1,4 +1,4 @@
-from typing import Optional
+from typing import Literal, Optional, TypeAlias
from bitbots_utils.utils import get_parameters_from_other_node
from rclpy.duration import Duration
@@ -7,6 +7,15 @@
from bitbots_blackboard.capsules import AbstractBlackboardCapsule
from bitbots_msgs.msg import Audio, HeadMode, RobotControlState
+THeadMode: TypeAlias = Literal[ # type: ignore[valid-type]
+ HeadMode.BALL_MODE,
+ HeadMode.FIELD_FEATURES,
+ HeadMode.LOOK_FORWARD,
+ HeadMode.DONT_MOVE,
+ HeadMode.BALL_MODE_PENALTY,
+ HeadMode.LOOK_FRONT,
+]
+
class MiscCapsule(AbstractBlackboardCapsule):
"""Capsule for miscellaneous functions."""
@@ -33,7 +42,10 @@ def __init__(self, node, blackboard):
# ## Tracking Part ##
#####################
- def set_head_duty(self, head_duty: int):
+ def set_head_duty(
+ self,
+ head_duty: THeadMode,
+ ) -> None:
head_duty_msg = HeadMode()
head_duty_msg.head_mode = head_duty
self.head_pub.publish(head_duty_msg)
@@ -42,7 +54,7 @@ def set_head_duty(self, head_duty: int):
# ## Robot state ##
###################
- def robot_state_callback(self, msg: RobotControlState):
+ def robot_state_callback(self, msg: RobotControlState) -> None:
self.robot_control_state = msg
def is_currently_walking(self) -> bool:
@@ -52,7 +64,7 @@ def is_currently_walking(self) -> bool:
# ## Timer ##
#############
- def start_timer(self, timer_name: str, duration_secs: int):
+ def start_timer(self, timer_name: str, duration_secs: int) -> None:
"""
Starts a timer
:param timer_name: Name of the timer
@@ -61,7 +73,7 @@ def start_timer(self, timer_name: str, duration_secs: int):
"""
self.timers[timer_name] = self._node.get_clock().now() + Duration(seconds=duration_secs)
- def end_timer(self, timer_name: str):
+ def end_timer(self, timer_name: str) -> None:
"""
Ends a timer
:param timer_name: Name of the timer
diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py
index a621bf0a3..30754da64 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py
+++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py
@@ -15,7 +15,7 @@
# Type of pathfinding goal relative to the ball
-class BallGoalType(Enum):
+class BallGoalType(str, Enum):
GRADIENT = "gradient"
MAP = "map"
CLOSE = "close"
@@ -26,8 +26,8 @@ class PathfindingCapsule(AbstractBlackboardCapsule):
def __init__(self, node, blackboard):
super().__init__(node, blackboard)
- self.position_threshold: str = self._node.get_parameter("pathfinding_position_threshold").value
- self.orientation_threshold: str = self._node.get_parameter("pathfinding_orientation_threshold").value
+ self.position_threshold: float = self._node.get_parameter("pathfinding_position_threshold").value
+ self.orientation_threshold: float = self._node.get_parameter("pathfinding_orientation_threshold").value
self.direct_cmd_vel_pub = self._node.create_publisher(Twist, "cmd_vel", 1)
self.pathfinding_pub = self._node.create_publisher(PoseStamped, "goal_pose", 1)
@@ -41,20 +41,20 @@ def __init__(self, node, blackboard):
self._node, "bitbots_path_planning", ["controller.orient_to_goal_distance"]
)["controller.orient_to_goal_distance"]
- def publish(self, msg: PoseStamped):
+ def publish(self, msg: PoseStamped) -> None:
"""
Sends a goal to the pathfinding.
"""
self.goal = msg
self.pathfinding_pub.publish(msg)
- def get_goal(self) -> PoseStamped:
+ def get_goal(self) -> Optional[PoseStamped]:
"""
Returns the latest goal that was send to the pathfinding.
"""
return self.goal
- def cancel_goal(self):
+ def cancel_goal(self) -> None:
"""
This function cancels the current goal of the pathfinding,
which will stop sending cmd_vel messages to the walking.
@@ -62,7 +62,7 @@ def cancel_goal(self):
"""
self.pathfinding_cancel_pub.publish(Empty())
- def cmd_vel_cb(self, msg: Twist):
+ def cmd_vel_cb(self, msg: Twist) -> None:
self.current_cmd_vel = msg
def get_current_cmd_vel(self) -> Twist:
@@ -73,7 +73,7 @@ def get_current_cmd_vel(self) -> Twist:
"""
return self.current_cmd_vel
- def stop_walk(self):
+ def stop_walk(self) -> None:
"""
This function stops the walking. It does not cancel the current goal of the
pathfinding and the walking will start again if the pathfinding sends a new message.
@@ -86,7 +86,7 @@ def stop_walk(self):
# Publish the stop command
self.direct_cmd_vel_pub.publish(msg)
- def calculate_time_to_ball(self):
+ def calculate_time_to_ball(self) -> None:
"""
Calculates the time to ball and saves it in the team data capsule.
"""
diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py
index e22dafd38..3fea4f71f 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py
+++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py
@@ -1,4 +1,4 @@
-from typing import Dict, List, 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[TeamData] = {}
+ self.team_data: dict[int, TeamData] = {}
for i in range(1, 7):
self.team_data[i] = TeamData()
self.times_to_ball = dict()
@@ -65,8 +65,8 @@ def __init__(self, node, blackboard):
self.action_update: float = 0.0
# Config
- self.data_timeout: float = self._node.get_parameter("team_data_timeout").value
- self.ball_max_covariance: float = self._node.get_parameter("ball_max_covariance").value
+ self.data_timeout: float = float(self._node.get_parameter("team_data_timeout").value)
+ self.ball_max_covariance: float = float(self._node.get_parameter("ball_max_covariance").value)
def is_valid(self, data: TeamData) -> bool:
"""
@@ -78,7 +78,7 @@ def is_valid(self, data: TeamData) -> bool:
and data.state != TeamData.STATE_PENALIZED
)
- def is_goalie_handling_ball(self):
+ def is_goalie_handling_ball(self) -> bool:
"""Returns true if the goalie is going to the ball."""
data: TeamData
for data in self.team_data.values():
@@ -90,7 +90,7 @@ def is_goalie_handling_ball(self):
return True
return False
- def is_team_mate_kicking(self):
+ def is_team_mate_kicking(self) -> bool:
"""Returns true if one of the players in the own team is kicking."""
data: TeamData
for data in self.team_data.values():
@@ -98,7 +98,9 @@ def is_team_mate_kicking(self):
return True
return False
- def team_rank_to_ball(self, own_ball_distance: float, count_goalies: bool = True, use_time_to_ball: bool = False):
+ def team_rank_to_ball(
+ self, own_ball_distance: float, count_goalies: bool = True, use_time_to_ball: bool = False
+ ) -> int:
"""
Returns the rank of this robot compared to the team robots concerning ball distance.
@@ -132,7 +134,7 @@ def team_rank_to_ball(self, own_ball_distance: float, count_goalies: bool = True
return rank + 1
return len(distances) + 1
- def set_action(self, action: int):
+ def set_action(self, action: int) -> None:
"""Set the action of this robot
:param action: An action from bitbots_msgs/Strategy"""
@@ -143,14 +145,11 @@ def set_action(self, action: int):
def get_action(self) -> Tuple[int, float]:
return self.strategy.action, self.action_update
- def set_role(self, role: str):
+ def set_role(self, role: Literal["goalie", "offense", "defense"]) -> None:
"""Set the role of this robot in the team
- :param role: String describing the role, possible values are:
- ['goalie', 'offense', 'defense']
+ :param role: String describing the role.
"""
- assert role in ["goalie", "offense", "defense"]
-
self.role = role
self.strategy.role = self.roles_mapping[role]
self.role_update = self._node.get_clock().now().nanoseconds / 1e9
@@ -158,8 +157,10 @@ def set_role(self, role: str):
def get_role(self) -> Tuple[int, float]:
return self.strategy.role, self.role_update
- def set_kickoff_strategy(self, strategy: int):
- assert strategy in [Strategy.SIDE_LEFT, Strategy.SIDE_MIDDLE, Strategy.SIDE_RIGHT]
+ def set_kickoff_strategy(
+ 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
@@ -209,14 +210,14 @@ def team_data_callback(self, msg: TeamData):
# Save team data
self.team_data[msg.robot_id] = msg
- def publish_strategy(self):
+ def publish_strategy(self) -> None:
"""Publish for team comm"""
self.strategy_sender.publish(self.strategy)
def publish_time_to_ball(self):
self.time_to_ball_publisher.publish(Float32(data=self.own_time_to_ball))
- def teammate_ball_is_valid(self):
+ def teammate_ball_is_valid(self) -> bool:
"""Returns true if a teammate has seen the ball accurately enough"""
return self.get_teammate_ball() is not None
@@ -227,7 +228,7 @@ def get_teammate_ball(self) -> Optional[PointStamped]:
team_data_infos = filter(self.is_valid, self.team_data.values())
def is_ball_good_enough(team_data: TeamData) -> bool:
- return (
+ return bool(
team_data.ball_absolute.covariance[0] < self.ball_max_covariance
and team_data.ball_absolute.covariance[7] < self.ball_max_covariance
)
@@ -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 --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py
index 47c0ef3d8..6f382ee8c 100644
--- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py
+++ b/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:
@@ -171,38 +183,38 @@ def forget_ball(self) -> None:
# Goal #
########
- def get_map_based_opp_goal_center_uv(self):
+ def get_map_based_opp_goal_center_uv(self) -> Tuple[float, float]:
x, y = self.get_map_based_opp_goal_center_xy()
return self.get_uv_from_xy(x, y)
- def get_map_based_opp_goal_center_xy(self):
+ def get_map_based_opp_goal_center_xy(self) -> Tuple[float, float]:
return self.field_length / 2, 0.0
- def get_map_based_own_goal_center_uv(self):
+ def get_map_based_own_goal_center_uv(self) -> Tuple[float, float]:
x, y = self.get_map_based_own_goal_center_xy()
return self.get_uv_from_xy(x, y)
- def get_map_based_own_goal_center_xy(self):
+ def get_map_based_own_goal_center_xy(self) -> Tuple[float, float]:
return -self.field_length / 2, 0.0
- def get_map_based_opp_goal_angle_from_ball(self):
+ def get_map_based_opp_goal_angle_from_ball(self) -> float:
ball_x, ball_y = self.get_ball_position_xy()
goal_x, goal_y = self.get_map_based_opp_goal_center_xy()
return math.atan2(goal_y - ball_y, goal_x - ball_x)
- def get_map_based_opp_goal_distance(self):
+ def get_map_based_opp_goal_distance(self) -> float:
x, y = self.get_map_based_opp_goal_center_xy()
return self.get_distance_to_xy(x, y)
- def get_map_based_opp_goal_angle(self):
+ def get_map_based_opp_goal_angle(self) -> float:
x, y = self.get_map_based_opp_goal_center_uv()
return math.atan2(y, x)
- def get_map_based_opp_goal_left_post_uv(self):
+ def get_map_based_opp_goal_left_post_uv(self) -> Tuple[float, float]:
x, y = self.get_map_based_opp_goal_center_xy()
return self.get_uv_from_xy(x, y - self.goal_width / 2)
- def get_map_based_opp_goal_right_post_uv(self):
+ def get_map_based_opp_goal_right_post_uv(self) -> Tuple[float, float]:
x, y = self.get_map_based_opp_goal_center_xy()
return self.get_uv_from_xy(x, y + self.goal_width / 2)
@@ -210,23 +222,21 @@ def get_map_based_opp_goal_right_post_uv(self):
# 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,13 +255,13 @@ 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 #
##########
- def get_uv_from_xy(self, x, y) -> Tuple[float, float]:
+ def get_uv_from_xy(self, x: float, y: float) -> Tuple[float, float]:
"""Returns the relativ positions of the robot to this absolute position"""
current_position = self.get_current_position()
x2 = x - current_position[0]
@@ -261,14 +271,14 @@ def get_uv_from_xy(self, x, y) -> Tuple[float, float]:
v = math.cos(theta) * y2 - math.sin(theta) * x2
return u, v
- def get_xy_from_uv(self, u, v):
+ def get_xy_from_uv(self, u: float, v: float) -> Tuple[float, float]:
"""Returns the absolute position from the given relative position to the robot"""
pos_x, pos_y, theta = self.get_current_position()
angle = math.atan2(v, u) + theta
hypotenuse = math.hypot(u, v)
return pos_x + math.sin(angle) * hypotenuse, pos_y + math.cos(angle) * hypotenuse
- def get_distance_to_xy(self, x, y):
+ def get_distance_to_xy(self, x: float, y: float) -> float:
"""Returns distance from robot to given position"""
u, v = self.get_uv_from_xy(x, y)
dist = math.hypot(u, v)
diff --git a/bitbots_behavior/bitbots_blackboard/mypy.ini b/bitbots_behavior/bitbots_blackboard/mypy.ini
new file mode 100644
index 000000000..6b19e8374
--- /dev/null
+++ b/bitbots_behavior/bitbots_blackboard/mypy.ini
@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True
diff --git a/bitbots_behavior/bitbots_blackboard/package.xml b/bitbots_behavior/bitbots_blackboard/package.xml
index 9463c26f2..4bbed0ca3 100644
--- a/bitbots_behavior/bitbots_blackboard/package.xml
+++ b/bitbots_behavior/bitbots_blackboard/package.xml
@@ -37,6 +37,10 @@
std_srvs
tf2_geometry_msgs
tf2
+ ament_mypy
+ ament_cmake_mypy
+
+ ament_cmake
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/__init__.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/__init__.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/__init__.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/__init__.py
index e69de29bb..2b0d3a359 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/__init__.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/__init__.py
@@ -0,0 +1,6 @@
+# Setting up runtime type checking for this package
+# We need to do this again here because the dsd imports
+# the decisions and actions from this package in a standalone way
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to.py
index e879b7d2c..72068ebbd 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to.py
+++ b/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 --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/turn.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/turn.py
index 79786dd23..63ae26d99 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/turn.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/turn.py
@@ -59,7 +59,7 @@ def __init__(self, blackboard, dsd, parameters):
self.max_speed = parameters.get("max_speed", 0.4)
# Check if the have a duration
- self.duration: Optional[float] = parameters.get("duration", None)
+ self.duration: Optional[float] = float(parameters.get("duration", None))
self.start_time = self.blackboard.node.get_clock().now()
def perform(self, reevaluate=False):
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/__init__.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/__init__.py
index e69de29bb..2b0d3a359 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/__init__.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/__init__.py
@@ -0,0 +1,6 @@
+# Setting up runtime type checking for this package
+# We need to do this again here because the dsd imports
+# the decisions and actions from this package in a standalone way
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/aligned_to_goal.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/aligned_to_goal.py
index c403adcef..3ae2f8854 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/aligned_to_goal.py
+++ b/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 --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/current_score.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/current_score.py
index 4d3746787..ac29d5fa8 100644
--- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/current_score.py
+++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/current_score.py
@@ -9,7 +9,7 @@ def __init__(self, blackboard, dsd, parameters):
super().__init__(blackboard, dsd, parameters)
def perform(self, reevaluate=False):
- own_goals = self.blackboard.gamestate.get_own_goals()
+ own_goals = self.blackboard.gamestate.get_our_goals()
opp_goals = self.blackboard.gamestate.get_opp_goals()
if own_goals == opp_goals:
diff --git a/bitbots_behavior/bitbots_body_behavior/package.xml b/bitbots_behavior/bitbots_body_behavior/package.xml
index b660e9c5a..ed5935edf 100644
--- a/bitbots_behavior/bitbots_body_behavior/package.xml
+++ b/bitbots_behavior/bitbots_body_behavior/package.xml
@@ -19,6 +19,7 @@
bitbots_blackboard
bitbots_msgs
+ bitbots_tts
bitbots_utils
dynamic_stack_decider
game_controller_hl_interfaces
@@ -29,6 +30,7 @@
soccer_vision_3d_msgs
tf_transformations
tf2
+ ament_mypy
diff --git a/bitbots_behavior/bitbots_body_behavior/test/mypy.ini b/bitbots_behavior/bitbots_body_behavior/test/mypy.ini
new file mode 100644
index 000000000..6b19e8374
--- /dev/null
+++ b/bitbots_behavior/bitbots_body_behavior/test/mypy.ini
@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True
diff --git a/bitbots_behavior/bitbots_body_behavior/test/test_mypy.py b/bitbots_behavior/bitbots_body_behavior/test/test_mypy.py
new file mode 100644
index 000000000..3dacd6040
--- /dev/null
+++ b/bitbots_behavior/bitbots_body_behavior/test/test_mypy.py
@@ -0,0 +1,10 @@
+from pathlib import Path
+
+import pytest
+from ament_mypy.main import main
+
+
+@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"
diff --git a/bitbots_misc/bitbots_parameter_blackboard/bitbots_parameter_blackboard/game_settings.py b/bitbots_misc/bitbots_parameter_blackboard/bitbots_parameter_blackboard/game_settings.py
index 2aeb11345..3c679b86c 100755
--- a/bitbots_misc/bitbots_parameter_blackboard/bitbots_parameter_blackboard/game_settings.py
+++ b/bitbots_misc/bitbots_parameter_blackboard/bitbots_parameter_blackboard/game_settings.py
@@ -29,7 +29,7 @@
)
-def provide_config(path):
+def provide_config(path: str) -> dict:
"""
reads out the yaml you are asking for with the path parameter
:param path: filepath for your yaml
@@ -113,7 +113,7 @@ def check_new_value(new_value: str, value_type: Any, valid_options: Optional[lis
return True
-def ask_for_confirmation(question) -> bool:
+def ask_for_confirmation(question: str) -> bool:
result = None
prompt = " [Y/n] "
valid = {"": True, "y": True, "n": False}
diff --git a/bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py b/bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py
index 33230d76e..cf824a0b6 100644
--- a/bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py
+++ b/bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py
@@ -1,6 +1,9 @@
from typing import Optional
import tf2_ros as tf2
+
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
from builtin_interfaces.msg import Duration as DurationMsg
from builtin_interfaces.msg import Time as TimeMsg
from geometry_msgs.msg import TransformStamped
@@ -10,6 +13,8 @@
from bitbots_tf_buffer.cpp_wrapper import Buffer as CppBuffer
+beartype_this_package()
+
class Buffer(tf2.BufferCore, tf2.BufferInterface):
"""
diff --git a/bitbots_misc/bitbots_tts/bitbots_tts/__init__.py b/bitbots_misc/bitbots_tts/bitbots_tts/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_misc/bitbots_tts/bitbots_tts/__init__.py
+++ b/bitbots_misc/bitbots_tts/bitbots_tts/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_misc/bitbots_utils/bitbots_utils/__init__.py b/bitbots_misc/bitbots_utils/bitbots_utils/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_misc/bitbots_utils/bitbots_utils/__init__.py
+++ b/bitbots_misc/bitbots_utils/bitbots_utils/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_misc/bitbots_utils/bitbots_utils/transforms.py b/bitbots_misc/bitbots_utils/bitbots_utils/transforms.py
index f40d482d7..92b448418 100644
--- a/bitbots_misc/bitbots_utils/bitbots_utils/transforms.py
+++ b/bitbots_misc/bitbots_utils/bitbots_utils/transforms.py
@@ -1,4 +1,5 @@
import math
+from typing import Literal
import numpy as np
from geometry_msgs.msg import Quaternion
@@ -37,7 +38,7 @@ def sixd2quat(sixd):
return mat2quat(mat)
-def quat2fused(q, order="wxyz"):
+def quat2fused(q, order: Literal["xyzw", "wxyz"] = "wxyz"):
# Convert to numpy array if necessary
q = np.asarray(q)
@@ -46,8 +47,6 @@ def quat2fused(q, order="wxyz"):
q_xyzw = q
elif order == "wxyz":
q_xyzw = wxyz2xyzw(q)
- else:
- raise ValueError(f"Unknown quaternion order: {order}")
# Fused yaw of Quaternion
fused_yaw = 2.0 * math.atan2(
@@ -78,7 +77,7 @@ def quat2fused(q, order="wxyz"):
# Conversion: Fused angles (3D/4D) --> Quaternion (wxyz)
-def fused2quat(fused_roll, fused_pitch, fused_yaw, hemi):
+def fused2quat(fused_roll: float, fused_pitch: float, fused_yaw: float, hemi: bool) -> np.ndarray:
# Precalculate the sine values
sth = math.sin(fused_pitch)
sphi = math.sin(fused_roll)
@@ -115,7 +114,7 @@ def fused2quat(fused_roll, fused_pitch, fused_yaw, hemi):
return np.array([chalpha * chpsi, shalpha * chgampsi, shalpha * shgampsi, chalpha * shpsi]) # Order: (w,x,y,z)
-def compute_imu_orientation_from_world(robot_quat_in_world):
+def compute_imu_orientation_from_world(robot_quat_in_world) -> tuple[tuple[float, float, float], np.ndarray]:
# imu orientation has roll and pitch relative to gravity vector. yaw in world frame
# get global yaw
yrp_world_frame = quat2euler(robot_quat_in_world, axes="szxy")
@@ -126,5 +125,5 @@ def compute_imu_orientation_from_world(robot_quat_in_world):
return [rp[0], rp[1], 0], yaw_quat
-def quat_from_yaw(yaw: float) -> Quaternion:
+def quat_from_yaw(yaw: float | int) -> Quaternion:
return msgify(Quaternion, wxyz2xyzw(euler2quat(yaw, 0, 0, axes="szxy")))
diff --git a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py
index a0e346750..2a187488b 100644
--- a/bitbots_misc/bitbots_utils/bitbots_utils/utils.py
+++ b/bitbots_misc/bitbots_utils/bitbots_utils/utils.py
@@ -4,6 +4,7 @@
import rclpy
import yaml
from ament_index_python import get_package_share_directory
+from beartype import BeartypeConf, BeartypeStrategy, beartype
from rcl_interfaces.msg import Parameter as ParameterMsg
from rcl_interfaces.msg import ParameterType as ParameterTypeMsg
from rcl_interfaces.msg import ParameterValue as ParameterValueMsg
@@ -11,8 +12,11 @@
from rclpy.node import Node
from rclpy.parameter import PARAMETER_SEPARATOR_STRING, Parameter, parameter_value_to_python
+# Create a new @nobeartype decorator disabling type-checking.
+nobeartype = beartype(conf=BeartypeConf(strategy=BeartypeStrategy.O0))
-def read_urdf(robot_name):
+
+def read_urdf(robot_name: str) -> str:
urdf = os.popen(
f"xacro {get_package_share_directory(f'{robot_name}_description')}"
f"/urdf/robot.urdf use_fake_walk:=false sim_ns:=false"
@@ -20,7 +24,7 @@ def read_urdf(robot_name):
return urdf
-def load_moveit_parameter(robot_name):
+def load_moveit_parameter(robot_name: str) -> List[ParameterMsg]:
moveit_parameters = get_parameters_from_plain_yaml(
f"{get_package_share_directory(f'{robot_name}_moveit_config')}/config/kinematics.yaml",
"robot_description_kinematics.",
@@ -40,7 +44,7 @@ def load_moveit_parameter(robot_name):
return moveit_parameters
-def get_parameters_from_ros_yaml(node_name, parameter_file, use_wildcard):
+def get_parameters_from_ros_yaml(node_name: str, parameter_file: str, use_wildcard: bool) -> List[ParameterMsg]:
# Remove leading slash and namespaces
with open(parameter_file) as f:
param_file = yaml.safe_load(f)
@@ -66,7 +70,7 @@ def get_parameters_from_ros_yaml(node_name, parameter_file, use_wildcard):
return parse_parameter_dict(namespace="", parameter_dict=param_dict)
-def get_parameters_from_plain_yaml(parameter_file, namespace=""):
+def get_parameters_from_plain_yaml(parameter_file, namespace="") -> List[ParameterMsg]:
with open(parameter_file) as f:
param_dict = yaml.safe_load(f)
return parse_parameter_dict(namespace=namespace, parameter_dict=param_dict)
@@ -180,7 +184,7 @@ def set_parameters_of_other_node(
return [res.success for res in response.results]
-def parse_parameter_dict(*, namespace, parameter_dict):
+def parse_parameter_dict(*, namespace: str, parameter_dict: dict) -> list[ParameterMsg]:
parameters = []
for param_name, param_value in parameter_dict.items():
full_param_name = namespace + param_name
diff --git a/bitbots_misc/system_monitor/system_monitor/__init__.py b/bitbots_misc/system_monitor/system_monitor/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_misc/system_monitor/system_monitor/__init__.py
+++ b/bitbots_misc/system_monitor/system_monitor/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/__init__.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/__init__.py
+++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/animation_recording.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/animation_recording.py
index 3631eb9e4..075036004 100644
--- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/animation_recording.py
+++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/animation_recording.py
@@ -4,8 +4,9 @@
import math
import os
from copy import deepcopy
+from dataclasses import dataclass, field
from datetime import datetime
-from typing import Optional
+from typing import Any, Optional
import regex as re
from rclpy.action import ActionClient
@@ -17,18 +18,40 @@
from bitbots_msgs.srv import AddAnimation
+@dataclass
+class Keyframe:
+ """
+ Defines a keyframe of the recorded Animation
+ """
+
+ name: str
+ duration: float
+ pause: float
+ goals: dict[str, float]
+ torque: dict[str, int]
+
+ def as_dict(self) -> dict[str, Any]:
+ return {
+ "name": self.name,
+ "duration": self.duration,
+ "pause": self.pause,
+ "goals": self.goals,
+ "torque": self.torque,
+ }
+
+
+@dataclass
class AnimationData:
"""
Defines a current status of the recorded Animation
"""
- def __init__(self):
- self.key_frames: list[dict] = []
- self.author: str = "Unknown"
- self.description: str = "Edit me!"
- self.last_edited: datetime = datetime.now().isoformat(" ")
- self.name: str = "None yet"
- self.version: int = 0
+ author: str = "Unknown"
+ key_frames: list[Keyframe] = field(default_factory=list)
+ last_edited: str = datetime.now().isoformat(" ")
+ description: str = "Edit me!"
+ version: str = "0"
+ name: str = "None yet"
class Recorder:
@@ -48,7 +71,7 @@ def __init__(
self.hcm_record_mode_client = hcm_record_mode_client
self.save_step("Initial step", frozen=True)
- def get_keyframes(self) -> list[dict]:
+ def get_keyframes(self) -> list[Keyframe]:
"""
Gets the keyframes of the current animation
@@ -56,7 +79,7 @@ def get_keyframes(self) -> list[dict]:
"""
return self.current_state.key_frames
- def get_keyframe(self, name: str) -> Optional[dict]:
+ def get_keyframe(self, name: str) -> Optional[Keyframe]:
"""
Gets the keyframe with the given name
@@ -64,7 +87,7 @@ def get_keyframe(self, name: str) -> Optional[dict]:
:return: the keyframe with the given name
"""
# Get the first keyframe with the given name or None
- return next(filter(lambda key_frame: key_frame["name"] == name, self.get_keyframes()), None)
+ return next(filter(lambda key_frame: key_frame.name == name, self.get_keyframes()), None) # type: ignore[arg-type, union-attr]
def get_keyframe_index(self, name: str) -> Optional[int]:
"""
@@ -79,20 +102,20 @@ def get_keyframe_index(self, name: str) -> Optional[int]:
return None
return self.get_keyframes().index(key_frame)
- def get_meta_data(self) -> tuple[str, int, str, str]:
+ def get_meta_data(self) -> tuple[str, str, str, str]:
"""
Returns the meta data of the current animation
"""
data = self.current_state
return data.name, data.version, data.author, data.description
- def set_meta_data(self, name: str, version: int, author: str, description: str) -> None:
+ def set_meta_data(self, name: str, version: str, author: str, description: str) -> None:
self.current_state.name = name
self.current_state.version = version
self.current_state.author = author
self.current_state.description = description
- def save_step(self, description: str, state: Optional[dict] = None, frozen: bool = False) -> None:
+ def save_step(self, description: str, state: Optional[AnimationData] = None, frozen: bool = False) -> None:
"""Save the current state of the Animation
for later restoration by the undo-command
@@ -161,8 +184,15 @@ def record(
:param override: Wether or not to override the frame at the given position
:param frozen: if True, the step cannot be undone
"""
- frame = {"name": frame_name, "duration": duration, "pause": pause, "goals": motor_pos, "torque": motor_torque}
- new_frame = deepcopy(frame)
+ new_frame = deepcopy(
+ Keyframe(
+ name=frame_name,
+ duration=duration,
+ pause=pause,
+ goals=motor_pos,
+ torque=motor_torque,
+ )
+ )
if seq_pos is None:
self.current_state.key_frames.append(new_frame)
self.save_step(f"Appending new keyframe '{frame_name}'", frozen=frozen)
@@ -172,9 +202,8 @@ def record(
else:
self.current_state.key_frames[seq_pos] = new_frame
self.save_step(f"Overriding keyframe at position {seq_pos} with '{frame_name}'", frozen=frozen)
- return True
- def save_animation(self, path: str, file_name: Optional[str] = None) -> None:
+ def save_animation(self, path: str, file_name: Optional[str] = None) -> str:
"""Dumps all keyframe data to an animation .json file
:param path: The folder where the file should be saved
@@ -194,8 +223,8 @@ def save_animation(self, path: str, file_name: Optional[str] = None) -> None:
# Convert the angles from radians to degrees
keyframes = deepcopy(self.current_state.key_frames)
for kf in keyframes:
- for k, v in kf["goals"].items():
- kf["goals"][k] = math.degrees(v)
+ for k, v in kf.goals.items():
+ kf.goals[k] = math.degrees(v)
# Construct the animation dictionary with meta data and keyframes
animation_dict = {
@@ -213,7 +242,7 @@ def save_animation(self, path: str, file_name: Optional[str] = None) -> None:
return "Saving to '%s'" % path + ". Done."
- def load_animation(self, path: str) -> None:
+ def load_animation(self, path: str) -> Optional[str]:
"""Record command, load a animation '.json' file
:param path: path of the animation to load
@@ -223,23 +252,34 @@ def load_animation(self, path: str) -> None:
data = json.load(fp)
# Check if the file is a valid animation and convert the angles from degrees to radians
try:
+ keyframes: list[Keyframe] = []
for keyframe in data["keyframes"]:
- for k, v in keyframe["goals"].items():
- keyframe["goals"][k] = math.radians(v)
+ keyframes.append(
+ Keyframe(
+ name=keyframe["name"],
+ duration=keyframe["duration"],
+ pause=keyframe["pause"],
+ goals={k: math.radians(v) for k, v in keyframe["goals"].items()},
+ torque=keyframe["torque"],
+ )
+ )
except ValueError as e:
self._node.get_logger().error("Animation {} is corrupt:\n {}".format(path, e.message.partition("\n")[0]))
return "Animation {} is corrupt:\n {}".format(path, e.message.partition("\n")[0])
# Set the current state to the loaded animation
- self.current_state.key_frames = data["keyframes"]
- self.current_state.name = data["name"]
- self.current_state.version = data["version"] if "version" in data else 0
- self.current_state.last_edited = data["last_edited"] if "last_edited" in data else datetime.now().isoformat(" ")
- self.current_state.author = data["author"] if "author" in data else "Unknown"
- self.current_state.description = data["description"] if "description" in data else ""
+ self.current_state = AnimationData(
+ key_frames=keyframes,
+ name=data["name"],
+ version=data["version"] if "version" in data else "0",
+ last_edited=data["last_edited"] if "last_edited" in data else datetime.now().isoformat(" "),
+ author=data["author"] if "author" in data else "Unknown",
+ description=data["description"] if "description" in data else "",
+ )
# Save the current state
self.save_step(f"Loading of animation named '{data['name']}'")
+ return None
def play(self, from_frame: int = 0, until_frame: int = -1) -> tuple[str, bool]:
"""Plays (a range of) the current animation using the animation server
@@ -275,13 +315,13 @@ def play(self, from_frame: int = 0, until_frame: int = -1) -> tuple[str, bool]:
"last_edited": datetime.isoformat(datetime.now(), " "),
"author": self.current_state.author,
"description": self.current_state.description,
- "keyframes": deepcopy(self.current_state.key_frames),
+ "keyframes": deepcopy(list(map(Keyframe.as_dict, self.current_state.key_frames))),
}
# Convert the angles from radians to degrees
for key_frame in animation_dict["keyframes"]:
- for joint, v in key_frame["goals"].items():
- key_frame["goals"][joint] = math.degrees(v)
+ for joint, v in key_frame["goals"].items(): # type: ignore[index]
+ key_frame["goals"][joint] = math.degrees(v) # type: ignore[index]
self._node.get_logger().debug(f"Playing {len(animation_dict['keyframes'])} frames...")
@@ -317,7 +357,7 @@ def change_frame_order(self, new_order: list[str]) -> None:
"""Changes the order of the frames given an array of frame names"""
new_ordered_frames = [self.get_keyframe(frame_name) for frame_name in new_order]
assert None not in new_ordered_frames, "One of the given frame names does not exist"
- self.current_state.key_frames = new_ordered_frames
+ self.current_state.key_frames = new_ordered_frames # type: ignore[assignment]
self.save_step("Reordered frames")
def duplicate(self, frame_name: str) -> None:
@@ -329,15 +369,16 @@ def duplicate(self, frame_name: str) -> None:
assert index is not None, "The given frame name does not exist"
# Create a deep copy of the frame
new_frame = deepcopy(self.get_keyframe(frame_name))
+ assert new_frame is not None, "The given frame name does not exist (even less possible here)"
# Add suffix to the frame name if it already exists and increment the number instead of making it longer
- while new_frame["name"] in [frame["name"] for frame in self.current_state.key_frames]:
+ while new_frame.name in [frame.name for frame in self.current_state.key_frames]:
# Check if the frame name ends with "_copy_" and a number, if so increment the number
- if re.match(r".*?_copy_(\d+)", new_frame["name"]):
- new_frame["name"] = re.sub(
- r"(_copy_)(\d+)", lambda m: f"{m.group(1)}{int(m.group(2)) + 1}", new_frame["name"]
+ if re.match(r".*?_copy_(\d+)", new_frame.name):
+ new_frame.name = re.sub(
+ r"(_copy_)(\d+)", lambda m: f"{m.group(1)}{int(m.group(2)) + 1}", new_frame.name
)
else:
- new_frame["name"] = f"{frame_name}_copy_1"
+ new_frame.name = f"{frame_name}_copy_1"
# Insert the new frame after the original frame
self.current_state.key_frames.insert(index + 1, new_frame)
self.save_step(f"Duplicated Frame '{frame_name}'")
diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py
index d89806360..e3e6e1d70 100755
--- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py
+++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/record_ui.py
@@ -72,7 +72,7 @@ def __init__(self, context):
self.create_initialized_recorder()
# Initialize the window
- self._widget = QMainWindow()
+ self._widget = QMainWindow() # type: ignore
# Load XML ui definition
ui_file = os.path.join(get_package_share_directory("bitbots_animation_rqt"), "resource", "RecordUI.ui")
@@ -314,7 +314,7 @@ def connect_gui_callbacks(self) -> None:
self._widget.frameList.key_pressed.connect(self.delete)
self._widget.frameList.itemSelectionChanged.connect(self.frame_select)
- def play_walkready(self) -> None:
+ def play_walkready(self, _) -> None:
"""
Plays the walkready animation on the robot
"""
@@ -401,7 +401,7 @@ def save(self, new_location: bool = False) -> None:
else:
self._widget.statusBar.showMessage("There is nothing to save!")
- def open(self) -> None:
+ def open(self, _) -> None:
"""
Deletes all current frames and instead loads an animation from a json file
"""
@@ -453,7 +453,7 @@ def play_until(self):
Plays the animation up to a certain frame
"""
# Get the index of the selected frame
- end_index = self._recorder.get_keyframe_index(self._selected_frame) + 1
+ end_index = self._recorder.get_keyframe_index(self._selected_frame) + 1 # type: ignore[operator]
# Play the animation
status, success = self._recorder.play(until_frame=end_index)
@@ -482,7 +482,7 @@ def goto_next(self):
# Go to the frame in the UI
self._widget.frameList.setCurrentRow(next_frame_index)
- self._selected_frame = self._recorder.get_keyframes()[next_frame_index]["name"]
+ self._selected_frame = self._recorder.get_keyframes()[next_frame_index].name
self.react_to_frame_change()
def goto_init(self):
@@ -567,8 +567,10 @@ def record(self):
# Display a message
self._widget.statusBar.showMessage(f"Recorded frame {self._selected_frame}")
- # Update the frames in the UI
+ # Update the frames in the UI, but don't trigger the frame selection signal
+ self._widget.frameList.blockSignals(True)
self.update_frames()
+ self._widget.frameList.blockSignals(False)
def undo(self):
"""
@@ -661,13 +663,19 @@ def frame_select(self):
if selected_frame is not None:
# check if unrecorded changes would be lost
unrecorded_changes = []
- current_keyframe_goals = self._recorder.get_keyframe(self._selected_frame)["goals"]
+ current_keyframe = self._recorder.get_keyframe(self._selected_frame)
+ assert current_keyframe is not None, "Current keyframe not found"
for motor_name, text_field in self._motor_controller_text_fields.items():
- # Get the angle from the textfield
- angle = text_field.value()
- # compare with angles in current keyframe
- if not current_keyframe_goals[motor_name] == math.radians(angle):
+ # Check if the motor active state has changed
+ currently_active = (
+ self._motor_switcher_active_checkbox[motor_name].checkState(0) == Qt.CheckState.Checked
+ )
+ active_in_current_keyframe = motor_name in current_keyframe.goals
+ motor_active_changed = currently_active != active_in_current_keyframe
+ # Check if the motor goal has changed
+ motor_goal_changed = current_keyframe.goals.get(motor_name, 0) != math.radians(text_field.value())
+ if motor_active_changed or motor_goal_changed:
unrecorded_changes.append(motor_name)
# warn user about unrecorded changes
@@ -678,6 +686,10 @@ def frame_select(self):
sure = QMessageBox.question(self._widget, "Sure?", message, QMessageBox.Yes | QMessageBox.No)
# Cancel the open if the user does not want to discard the changes
if sure == QMessageBox.No:
+ # Update the UI to reselect the current frame (blocking signals to avoid infinite loop)
+ self._widget.frameList.blockSignals(True)
+ self._widget.frameList.setCurrentRow(self._recorder.get_keyframe_index(self._selected_frame))
+ self._widget.frameList.blockSignals(False)
return
# Update state so we have a new selected frame
self._selected_frame = selected_frame_name
@@ -691,6 +703,7 @@ def react_to_frame_change(self):
"""
# Get the selected frame
selected_frame = self._recorder.get_keyframe(self._selected_frame)
+ assert selected_frame is not None, "Selected frame not found in recorder"
# Update the name
self._widget.lineFrameName.setText(self._selected_frame)
@@ -698,28 +711,28 @@ def react_to_frame_change(self):
# Update what motors are active, what are stiff and set the angles accordingly
for motor_name in self.motors:
# Update checkbox if the motor is active or not
- active = motor_name in selected_frame["goals"]
+ active = motor_name in selected_frame.goals.keys()
self._motor_switcher_active_checkbox[motor_name].setCheckState(0, Qt.Checked if active else Qt.Unchecked)
# Update checkbox if the motor is stiff or not
- stiff = "torques" not in selected_frame or (
- motor_name in selected_frame["torques"] and bool(selected_frame["torques"][motor_name])
+ stiff = len(selected_frame.torque) == 0 or (
+ motor_name in selected_frame.torque and bool(selected_frame.torque[motor_name])
)
self._motor_controller_torque_checkbox[motor_name].setCheckState(0, Qt.Checked if stiff else Qt.Unchecked)
# Update the motor angle controls (value and active state)
if active:
self._motor_controller_text_fields[motor_name].setValue(
- round(math.degrees(selected_frame["goals"][motor_name]), 2)
+ round(math.degrees(selected_frame.goals[motor_name]), 2)
)
- self._working_angles[motor_name] = selected_frame["goals"][motor_name]
+ self._working_angles[motor_name] = selected_frame.goals[motor_name]
else:
self._motor_controller_text_fields[motor_name].setValue(0.0)
# Update the duration and pause
- self._widget.spinBoxDuration.setValue(selected_frame["duration"])
- self._widget.spinBoxPause.setValue(selected_frame["pause"])
+ self._widget.spinBoxDuration.setValue(selected_frame.duration)
+ self._widget.spinBoxPause.setValue(selected_frame.pause)
self.react_to_motor_selection()
@@ -773,7 +786,7 @@ def react_to_motor_selection(self):
def update_frames(self) -> None:
"""
- Updates the list of frames in the GUI based on the keyframes in the recorder
+ Updates the list of frames in the GUI based on the keyframes in the recorder'
"""
# Get the current state of the recorder and the index of the selected frame
keyframes = self._recorder.get_keyframes()
@@ -783,7 +796,7 @@ def update_frames(self) -> None:
if index is None:
# If it is not, select the first frame
index = 0
- self._selected_frame = keyframes[index]["name"]
+ self._selected_frame = keyframes[index].name
print(f"Selected frame {self._selected_frame}")
# Clear the list of frames
@@ -792,7 +805,7 @@ def update_frames(self) -> None:
# Add all frames to the list
for frame in keyframes:
item = QListWidgetItem()
- item.setText(frame["name"])
+ item.setText(frame.name)
self._widget.frameList.addItem(item)
# Select the correct frame
diff --git a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/utils.py b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/utils.py
index f9f46c05a..6217bed78 100644
--- a/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/utils.py
+++ b/bitbots_motion/bitbots_animation_rqt/bitbots_animation_rqt/utils.py
@@ -1,4 +1,4 @@
-from typing import Callable
+from typing import Any, Callable
from PyQt5.QtCore import QObject, Qt, pyqtSignal
from PyQt5.QtWidgets import QListWidget, QWidget
@@ -28,12 +28,12 @@ def dropEvent(self, e): # noqa: N802
# fmt: off
def keyPressEvent(self, event): # noqa: N802
# fmt: on
- if event.key() == Qt.Key_Delete:
+ if event.key() == Qt.Key_Delete: # type: ignore[attr-defined]
super().keyPressEvent(event)
self.key_pressed.emit()
- elif event.key() == Qt.Key_Up and self.currentRow() - 1 >= 0:
+ elif event.key() == Qt.Key_Up and self.currentRow() - 1 >= 0: # type: ignore[attr-defined]
self.setCurrentRow(self.currentRow() - 1)
- elif event.key() == Qt.Key_Down and self.currentRow() + 1 < self.count():
+ elif event.key() == Qt.Key_Down and self.currentRow() + 1 < self.count(): # type: ignore[attr-defined]
self.setCurrentRow(self.currentRow() + 1)
@@ -43,7 +43,7 @@ class JointStateCommunicate(QObject):
def flatten_dict(input_dict: dict, parent_key: str = "", sep: str = ".") -> dict:
"""Flatten a nested dictionary."""
- items = []
+ items: list[Any] = []
for k, v in input_dict.items():
new_key = f"{parent_key}{sep}{k}" if parent_key else k
if isinstance(v, dict):
diff --git a/bitbots_motion/bitbots_animation_rqt/package.xml b/bitbots_motion/bitbots_animation_rqt/package.xml
index aec6962c0..f8af2aefb 100644
--- a/bitbots_motion/bitbots_animation_rqt/package.xml
+++ b/bitbots_motion/bitbots_animation_rqt/package.xml
@@ -21,6 +21,7 @@
rqt_gui_py
rqt_gui
std_srvs
+ ament_mypy
diff --git a/bitbots_motion/bitbots_animation_rqt/setup.py b/bitbots_motion/bitbots_animation_rqt/setup.py
index ff22687d2..45e680dd3 100644
--- a/bitbots_motion/bitbots_animation_rqt/setup.py
+++ b/bitbots_motion/bitbots_animation_rqt/setup.py
@@ -13,6 +13,7 @@
],
install_requires=["setuptools"],
zip_safe=True,
+ tests_require=["pytest"],
entry_points={
"console_scripts": [
"animation_gui = " + package_name + ".record_ui:main",
diff --git a/bitbots_motion/bitbots_animation_rqt/test/mypy.ini b/bitbots_motion/bitbots_animation_rqt/test/mypy.ini
new file mode 100644
index 000000000..b7b297c6a
--- /dev/null
+++ b/bitbots_motion/bitbots_animation_rqt/test/mypy.ini
@@ -0,0 +1,6 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True
+disable_error_code = attr-defined
diff --git a/bitbots_motion/bitbots_animation_rqt/test/test_mypy.py b/bitbots_motion/bitbots_animation_rqt/test/test_mypy.py
new file mode 100644
index 000000000..3dacd6040
--- /dev/null
+++ b/bitbots_motion/bitbots_animation_rqt/test/test_mypy.py
@@ -0,0 +1,10 @@
+from pathlib import Path
+
+import pytest
+from ament_mypy.main import main
+
+
+@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"
diff --git a/bitbots_motion/bitbots_animation_server/CMakeLists.txt b/bitbots_motion/bitbots_animation_server/CMakeLists.txt
deleted file mode 100644
index 988d53714..000000000
--- a/bitbots_motion/bitbots_animation_server/CMakeLists.txt
+++ /dev/null
@@ -1,13 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(bitbots_animation_server)
-
-find_package(bitbots_docs REQUIRED)
-find_package(rclpy REQUIRED)
-find_package(ament_cmake REQUIRED)
-find_package(ament_cmake_python REQUIRED)
-
-enable_bitbots_docs()
-
-ament_python_install_package(${PROJECT_NAME})
-
-ament_package()
diff --git a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/__init__.py b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/__init__.py
+++ b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation.py b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation.py
index fb525677c..55c456deb 100644
--- a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation.py
+++ b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation.py
@@ -44,9 +44,9 @@ def parse(info: dict) -> Animation:
This method is parsing an animation from a :class:`dict`
instance *info*, as created by :func:`as_dict`.
"""
- anim = Animation(info["name"], ())
+ anim = Animation(info["name"], [])
- keyframes = info.get("keyframes", ())
+ keyframes = info.get("keyframes", [])
anim.keyframes = [
Keyframe(
k.get("goals", {}),
@@ -59,24 +59,3 @@ def parse(info: dict) -> Animation:
]
return anim
-
-
-def as_dict(anim: Animation) -> dict:
- """
- Convert an animation to builtin python types to
- make it serializable to formats like ``json``.
- """
- return {
- "name": anim.name,
- "interpolators": {n: ip.__name__ for n, ip in anim.interpolators},
- "keyframes": [
- {
- "duration": k.duration,
- "pause": k.pause,
- "goals": k.goals,
- "torque": k.torque,
- "stabilization_functions": k.stabilization_functions,
- }
- for k in anim.keyframes
- ],
- }
diff --git a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation_node.py b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation_node.py
index f7cefea55..d9c473ae7 100755
--- a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation_node.py
+++ b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/animation_node.py
@@ -27,7 +27,7 @@
from bitbots_msgs.srv import AddAnimation
# A hashable definition for the UUID of the goals
-UUID = tuple[int]
+UUID = tuple[np.uint8, ...]
class AnimationNode(Node):
@@ -165,7 +165,7 @@ def finish(successful: bool) -> PlayAnimation.Result:
# Send request to make the HCM to go into animation play mode
num_tries = 0
- while rclpy.ok() and (not self.hcm_animation_mode.call(SetBool.Request(data=True)).success):
+ while rclpy.ok() and (not self.hcm_animation_mode.call(SetBool.Request(data=True)).success): # type: ignore[attr-defined]
if num_tries >= 10:
self.get_logger().error("Failed to request HCM to go into animation play mode")
return finish(successful=False)
@@ -263,7 +263,7 @@ def finish(successful: bool) -> PlayAnimation.Result:
if pose is None or (request.bounds and once and t > animator.get_keyframe_times()[request.end - 1]):
# Animation is finished, tell it to the hcm (except if it is from the hcm)
if not request.hcm:
- hcm_result = self.hcm_animation_mode.call(SetBool.Request(data=False))
+ hcm_result: SetBool.Response = self.hcm_animation_mode.call(SetBool.Request(data=False))
if not hcm_result.success:
self.get_logger().error(f"Failed to finish animation on HCM. Reason: {hcm_result.message}")
@@ -288,7 +288,7 @@ def finish(successful: bool) -> PlayAnimation.Result:
sys.exit(0)
return finish(successful=False)
- def update_current_pose(self, msg) -> None:
+ def update_current_pose(self, msg: JointState) -> None:
"""Gets the current motor positions and updates the representing pose accordingly."""
self.current_joint_states = msg
@@ -296,7 +296,7 @@ def imu_callback(self, msg: Imu) -> None:
"""Callback for the IMU data."""
self.imu_data = msg
- def send_animation(self, from_hcm: bool, pose: dict, torque: Optional[dict]):
+ def send_animation(self, from_hcm: bool, pose: dict[str, float], torque: Optional[dict[str, float]] = None) -> None:
"""Sends an animation to the hcm"""
self.hcm_publisher.publish(
AnimationMsg(
@@ -309,8 +309,8 @@ def send_animation(self, from_hcm: bool, pose: dict, torque: Optional[dict]):
positions=pose.values(),
velocities=[-1.0] * len(pose),
accelerations=[-1.0] * len(pose),
- max_currents=[np.clip((torque[joint]), 0.0, 1.0) for joint in pose]
- if torque and False # TODO
+ max_currents=[np.clip((torque[joint]), 0.0, 1.0) for joint in pose.keys()] # type: ignore[index]
+ if torque and False
else [-1.0] * len(pose), # fmt: skip
),
)
diff --git a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/resource_manager.py b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/resource_manager.py
index e7436e403..315f23466 100644
--- a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/resource_manager.py
+++ b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/resource_manager.py
@@ -18,9 +18,9 @@ def __init__(self, robot_type: str):
# Get the path to the animations folder
self.basepath = abspath(os.path.join(get_package_share_directory(robot_type + "_animations"), "animations"))
- self.cache = {}
- self.files = []
- self.names = [] # Plain animation names, without filename-extension
+ self.cache: dict[str, str] = {}
+ self.files: list[str] = []
+ self.names: list[str] = [] # Plain animation names, without filename-extension
self.animpath = self._get_animpath()
def search(self, path, folders, filename=""):
@@ -121,7 +121,7 @@ def find_resource(self, name):
"""Finds a resource relative to self.basepath"""
return self.find(name)
- def _get_animpath(self):
+ def _get_animpath(self) -> list[str]:
"""
Get a list of folders in the animations/ folder.
"""
diff --git a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/spline_animator.py b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/spline_animator.py
index b2bb84e75..6dffb7649 100644
--- a/bitbots_motion/bitbots_animation_server/bitbots_animation_server/spline_animator.py
+++ b/bitbots_motion/bitbots_animation_server/bitbots_animation_server/spline_animator.py
@@ -74,8 +74,8 @@ def get_positions_rad(self, time):
return ret_dict
- def get_torque(self, current_time):
- torque = {}
+ def get_torque(self, current_time) -> dict[str, float]:
+ torque: dict[str, float] = {}
if current_time < 0 or current_time > self.animation_duration:
return torque
diff --git a/bitbots_motion/bitbots_animation_server/package.xml b/bitbots_motion/bitbots_animation_server/package.xml
index 5552f2810..a1e94693e 100644
--- a/bitbots_motion/bitbots_animation_server/package.xml
+++ b/bitbots_motion/bitbots_animation_server/package.xml
@@ -21,6 +21,7 @@
bitbots_utils
rclpy
std_msgs
+ ament_mypy
diff --git a/bitbots_motion/bitbots_animation_server/setup.py b/bitbots_motion/bitbots_animation_server/setup.py
index c93d21760..7a1b78643 100644
--- a/bitbots_motion/bitbots_animation_server/setup.py
+++ b/bitbots_motion/bitbots_animation_server/setup.py
@@ -21,6 +21,7 @@
zip_safe=True,
keywords=["ROS"],
license="MIT",
+ tests_require=["pytest"],
entry_points={
"console_scripts": [
"animation_node = bitbots_animation_server.animation_node:main",
diff --git a/bitbots_motion/bitbots_animation_server/test/mypy.ini b/bitbots_motion/bitbots_animation_server/test/mypy.ini
new file mode 100644
index 000000000..6b19e8374
--- /dev/null
+++ b/bitbots_motion/bitbots_animation_server/test/mypy.ini
@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True
diff --git a/bitbots_motion/bitbots_animation_server/test/test_mypy.py b/bitbots_motion/bitbots_animation_server/test/test_mypy.py
new file mode 100644
index 000000000..3dacd6040
--- /dev/null
+++ b/bitbots_motion/bitbots_animation_server/test/test_mypy.py
@@ -0,0 +1,10 @@
+from pathlib import Path
+
+import pytest
+from ament_mypy.main import main
+
+
+@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"
diff --git a/bitbots_motion/bitbots_hcm/CMakeLists.txt b/bitbots_motion/bitbots_hcm/CMakeLists.txt
index 9e427ccae..4fa8c90a9 100644
--- a/bitbots_motion/bitbots_hcm/CMakeLists.txt
+++ b/bitbots_motion/bitbots_hcm/CMakeLists.txt
@@ -58,6 +58,9 @@ if(BUILD_TESTING)
ament_add_pytest_test(
hcm_py_test test/pytest PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" APPEND_ENV
PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR})
+
+ find_package(ament_cmake_mypy REQUIRED)
+ ament_mypy(CONFIG_FILE "${CMAKE_CURRENT_LIST_DIR}/mypy.ini")
endif()
ament_python_install_package(${PROJECT_NAME})
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/__init__.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_motion/bitbots_hcm/bitbots_hcm/__init__.py
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/__init__.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/__init__.py
index 25bdf4bbe..61a6ccd0c 100644
--- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/__init__.py
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/__init__.py
@@ -1,7 +1,13 @@
+# Setting up runtime type checking for this package
+# We need to do this again here because the dsd imports
+# the decisions and actions from this package in a standalone way
+from beartype.claw import beartype_this_package
from dynamic_stack_decider.abstract_action_element import AbstractActionElement
from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard
+beartype_this_package()
+
class AbstractHCMActionElement(AbstractActionElement):
"""
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py
index 5beebe5fe..882ae4624 100644
--- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py
@@ -88,6 +88,9 @@ def animation_feedback_cb(self, msg):
self.publish_debug_data("Animation Percent Done", str(feedback.percent_done))
def animation_finished(self):
+ assert (
+ self.blackboard.animation_action_current_goal is not None
+ ), "No animation action goal set, so we cannot check if it is finished"
return (
self.blackboard.animation_action_current_goal.done()
and self.blackboard.animation_action_current_goal.result().status
@@ -202,7 +205,7 @@ def on_pop(self):
Cancel the current goal when the action is popped
"""
super().on_pop()
- if not self.animation_finished():
+ if self.blackboard.dynup_action_current_goal is not None and not self.animation_finished():
self.blackboard.dynup_action_current_goal.result().cancel_goal_async()
def start_animation(self):
@@ -245,6 +248,9 @@ def animation_feedback_cb(self, msg):
self.publish_debug_data("Dynup Percent Done", str(feedback.percent_done))
def animation_finished(self):
+ assert (
+ self.blackboard.dynup_action_current_goal is not None
+ ), "No dynup action goal set, so we cannot check if it is finished"
return (
self.blackboard.dynup_action_current_goal.done()
and self.blackboard.dynup_action_current_goal.result().status
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/state.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/state.py
index b58b4231e..4c2804302 100644
--- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/state.py
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/state.py
@@ -1,6 +1,7 @@
from abc import ABC, abstractmethod
from bitbots_hcm.hcm_dsd.actions import AbstractHCMActionElement
+from bitbots_hcm.type_utils import T_RobotControlState
from bitbots_msgs.msg import RobotControlState
@@ -11,7 +12,7 @@ class AbstractRobotState(AbstractHCMActionElement, ABC):
"""
@abstractmethod
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
"""
Returns the state which should be set. This will be implemented by the subclasses.
"""
@@ -27,65 +28,65 @@ def perform(self, reevaluate=False):
class RobotStateStartup(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.STARTUP
class RobotStateControllable(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.CONTROLLABLE
class RobotStateWalking(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.WALKING
class RobotStateAnimationRunning(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.ANIMATION_RUNNING
class RobotStatePickedUp(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.PICKED_UP
class RobotStateFalling(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.FALLING
class RobotStateFallen(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.FALLEN
class RobotStateGettingUp(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.GETTING_UP
class RobotStatePenalty(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.PENALTY
class RobotStateRecord(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.RECORD
class RobotStateKicking(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.KICKING
class RobotStateHardwareProblem(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.HARDWARE_PROBLEM
class RobotStateMotorOff(AbstractRobotState):
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
return RobotControlState.MOTOR_OFF
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/__init__.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/__init__.py
index 82101034e..e79e007a9 100644
--- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/__init__.py
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/__init__.py
@@ -1,7 +1,13 @@
+# Setting up runtime type checking for this package
+# We need to do this again here because the dsd imports
+# the decisions and actions from this package in a standalone way
+from beartype.claw import beartype_this_package
from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement
from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard
+beartype_this_package()
+
class AbstractHCMDecisionElement(AbstractDecisionElement):
"""
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py
index c770b8326..c7072a5c0 100644
--- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/animation.py
@@ -29,6 +29,9 @@ def perform(self, reevaluate=False):
# We can safely assume that the last animation start time is set because the animation is running
# Calculate time since last animation start
+ assert (
+ self.blackboard.last_animation_start_time is not None
+ ), "Last animation start time is not set, this should be impossible"
time_delta_since_last_animation_start = (
self.blackboard.node.get_clock().now().nanoseconds / 1e9
- self.blackboard.last_animation_start_time.nanoseconds / 1e9
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/check_hardware.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/check_hardware.py
index e36034297..2b584e439 100644
--- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/check_hardware.py
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/check_hardware.py
@@ -126,10 +126,14 @@ def perform(self, reevaluate=False):
else:
return "OKAY"
- if self.blackboard.previous_imu_msg is None or (
- self.blackboard.node.get_clock().now().nanoseconds
- - self.blackboard.last_different_imu_state_time.nanoseconds
- > self.blackboard.imu_timeout_duration * 1e9
+ if (
+ self.blackboard.previous_imu_msg is None
+ or self.blackboard.last_different_imu_state_time is None
+ or (
+ self.blackboard.node.get_clock().now().nanoseconds
+ - self.blackboard.last_different_imu_state_time.nanoseconds
+ > self.blackboard.imu_timeout_duration * 1e9
+ )
):
if (
self.blackboard.current_state == RobotControlState.STARTUP
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/falling.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/falling.py
index 44a421875..e8f7005b1 100644
--- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/falling.py
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/decisions/falling.py
@@ -4,6 +4,7 @@
import numpy as np
from bitbots_utils.transforms import quat2fused
from rclpy.duration import Duration
+from rclpy.time import Time
from bitbots_hcm.hcm_dsd.decisions import AbstractHCMDecisionElement
@@ -32,7 +33,7 @@ def __init__(self, blackboard, dsd, parameters):
self.smoothing = self.blackboard.node.get_parameter("smooth_threshold").value
# Initialize smoothing list that stores the last results
- self.smoothing_list: list[FallDirection] = []
+ self.smoothing_list: list[tuple[Time, FallDirection]] = []
def perform(self, reevaluate=False):
"""Checks if the robot is currently falling and in which direction."""
@@ -77,7 +78,7 @@ def perform(self, reevaluate=False):
# Prune old elements from smoothing history
self.smoothing_list = list(
filter(
- lambda x: x[0] > self.blackboard.node.get_clock().now() - Duration(seconds=self.smoothing),
+ lambda x: x[0] > self.blackboard.node.get_clock().now() - Duration(seconds=self.smoothing), # type: ignore[arg-type]
self.smoothing_list,
)
)
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py
index 3bbdaea14..b0ae2b7e9 100644
--- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm_blackboard.py
@@ -12,6 +12,7 @@
from std_srvs.srv import Empty as EmptySrv
from std_srvs.srv import SetBool
+from bitbots_hcm.type_utils import T_RobotControlState
from bitbots_msgs.action import Dynup, PlayAnimation
from bitbots_msgs.msg import Audio, JointTorque, RobotControlState
from bitbots_msgs.srv import SetTeachingMode
@@ -22,7 +23,7 @@ def __init__(self, node: Node):
self.node = node
# Basic state
- self.current_state: RobotControlState = RobotControlState.STARTUP
+ self.current_state: T_RobotControlState = RobotControlState.STARTUP
self.stopped: bool = False
# Save start time
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py
index b35ea8e71..a0bc0ec88 100755
--- a/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/humanoid_control_module.py
@@ -24,6 +24,7 @@
from bitbots_hcm import hcm_dsd
from bitbots_hcm.hcm_dsd.hcm_blackboard import HcmBlackboard
+from bitbots_hcm.type_utils import T_RobotControlState
from bitbots_msgs.msg import FootPressure, RobotControlState
from bitbots_msgs.srv import ManualPenalize, SetTeachingMode
@@ -34,18 +35,15 @@ def __init__(self, use_sim_time, simulation_active, visualization_active):
node_name = "hcm_py"
# Load parameters from yaml file because this is a hacky cpp python hybrid node for performance reasons
- parameter_msgs: list(ParameterMsg) = get_parameters_from_ros_yaml(
+ parameter_msgs: list[ParameterMsg] = get_parameters_from_ros_yaml(
node_name, f"{get_package_share_directory('bitbots_hcm')}/config/hcm_wolfgang.yaml", use_wildcard=True
)
- parameters = []
- for parameter_msg in parameter_msgs:
- parameters.append(Parameter.from_parameter_msg(parameter_msg))
- if use_sim_time:
- parameters.append(Parameter("use_sim_time", type_=Parameter.Type.BOOL, value=True))
- if simulation_active:
- parameters.append(Parameter("simulation_active", type_=Parameter.Type.BOOL, value=True))
- if visualization_active:
- parameters.append(Parameter("visualization_active", type_=Parameter.Type.BOOL, value=True))
+ parameters = [
+ Parameter("use_sim_time", type_=Parameter.Type.BOOL, value=use_sim_time),
+ Parameter("simulation_active", type_=Parameter.Type.BOOL, value=simulation_active),
+ Parameter("visualization_active", type_=Parameter.Type.BOOL, value=visualization_active),
+ ]
+ parameters.extend(map(Parameter.from_parameter_msg, parameter_msgs))
# Create Python node
self.node = Node(
@@ -161,7 +159,7 @@ def diag_cb(self, msg: DiagnosticArray):
elif "/Pressure" in status.name:
self.blackboard.pressure_diag_error = status.level in (DiagnosticStatus.ERROR, DiagnosticStatus.STALE)
- def get_state(self) -> RobotControlState:
+ def get_state(self) -> T_RobotControlState:
"""Returns the current state of the HCM."""
return self.blackboard.current_state
diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/type_utils.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/type_utils.py
new file mode 100644
index 000000000..20bd175fc
--- /dev/null
+++ b/bitbots_motion/bitbots_hcm/bitbots_hcm/type_utils.py
@@ -0,0 +1,21 @@
+from typing import Literal, TypeAlias
+
+from bitbots_msgs.msg import RobotControlState
+
+T_RobotControlState: TypeAlias = Literal[ # type: ignore[valid-type]
+ RobotControlState.CONTROLLABLE,
+ RobotControlState.FALLING,
+ RobotControlState.FALLEN,
+ RobotControlState.GETTING_UP,
+ RobotControlState.ANIMATION_RUNNING,
+ RobotControlState.STARTUP,
+ RobotControlState.SHUTDOWN,
+ RobotControlState.PENALTY,
+ RobotControlState.RECORD,
+ RobotControlState.WALKING,
+ RobotControlState.MOTOR_OFF,
+ RobotControlState.HCM_OFF,
+ RobotControlState.HARDWARE_PROBLEM,
+ RobotControlState.PICKED_UP,
+ RobotControlState.KICKING,
+]
diff --git a/bitbots_motion/bitbots_hcm/mypy.ini b/bitbots_motion/bitbots_hcm/mypy.ini
new file mode 100644
index 000000000..6b19e8374
--- /dev/null
+++ b/bitbots_motion/bitbots_hcm/mypy.ini
@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True
diff --git a/bitbots_motion/bitbots_hcm/package.xml b/bitbots_motion/bitbots_hcm/package.xml
index 9f6fd464a..9e90af219 100644
--- a/bitbots_motion/bitbots_hcm/package.xml
+++ b/bitbots_motion/bitbots_hcm/package.xml
@@ -9,7 +9,7 @@
Marc Bestmann
Hamburg Bit-Bots
-
+
MIT
Marc Bestmann
@@ -33,6 +33,7 @@
ros2_python_extension
sensor_msgs
std_msgs
+ ament_cmake_mypy
diff --git a/bitbots_motion/bitbots_splines/bitbots_splines/__init__.py b/bitbots_motion/bitbots_splines/bitbots_splines/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_motion/bitbots_splines/bitbots_splines/__init__.py
+++ b/bitbots_motion/bitbots_splines/bitbots_splines/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/__init__.py b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/__init__.py
+++ b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/__init__.py b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/__init__.py
index 47ca48a40..c6114ca3b 100644
--- a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/__init__.py
+++ b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/__init__.py
@@ -1,7 +1,11 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
from dynamic_stack_decider.abstract_action_element import AbstractActionElement
from bitbots_localization_handler.localization_dsd.localization_blackboard import LocalizationBlackboard
+beartype_this_package()
+
class AbstractLocalizationActionElement(AbstractActionElement):
"""
diff --git a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/initialize.py b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/initialize.py
index aa035280d..29b401efa 100644
--- a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/initialize.py
+++ b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/actions/initialize.py
@@ -60,6 +60,15 @@ def perform(self, reevaluate=False):
self.blackboard.node.get_logger().debug("Initializing position")
while not self.blackboard.reset_filter_proxy.wait_for_service(timeout_sec=3.0):
self.blackboard.node.get_logger().info("Localization reset service not available, waiting again...")
+
+ # Abort if we don't know the current position
+ if self.blackboard.robot_pose is None:
+ self.blackboard.node.get_logger().warn(
+ "Can't initialize position, because we don't know the current position"
+ )
+ return self.pop()
+
+ # Reset the localization to the current position
self.blackboard.reset_filter_proxy.call_async(
ResetFilter.Request(
init_mode=ResetFilter.Request.POSITION,
@@ -77,6 +86,13 @@ def perform(self, reevaluate=False):
while not self.blackboard.reset_filter_proxy.wait_for_service(timeout_sec=3.0):
self.blackboard.node.get_logger().info("Localization reset service not available, waiting again...")
+ # Abort if we don't know the current pose
+ if self.blackboard.robot_pose is None:
+ self.blackboard.node.get_logger().warn(
+ "Can't initialize position, because we don't know the current position"
+ )
+ return self.pop()
+
# Calculate the angle of the robot after the fall by adding the yaw difference during the fall
# (estimated by the IMU) to the last known localization yaw
esimated_angle = (
@@ -113,9 +129,7 @@ def perform(self, reevaluate=False):
self.do_not_reevaluate()
self.blackboard.node.get_logger().debug("Initializing on the side lines of our side")
while not self.blackboard.reset_filter_proxy.wait_for_service(timeout_sec=3.0):
- self.blackboard.reset_filter_proxy.node.get_logger().info(
- "Localization reset service not available, waiting again..."
- )
+ self.blackboard.node.get_logger().info("Localization reset service not available, waiting again...")
self.blackboard.reset_filter_proxy.call_async(
ResetFilter.Request(init_mode=ResetFilter.Request.POSE, x=float(-self.blackboard.field_length / 2), y=0.0)
)
@@ -143,6 +157,9 @@ class RedoLastInit(AbstractInitialize):
def __init__(self, blackboard, dsd, parameters):
super().__init__(blackboard, dsd, parameters)
+ if self.blackboard.last_init_action_type is None:
+ raise ValueError("There is no last init action to redo")
+
# Creates an instance of the last init action
self.sub_action: AbstractInitialize = self.blackboard.last_init_action_type(blackboard, dsd, parameters)
diff --git a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/decisions/__init__.py b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/decisions/__init__.py
index 99892af44..e2df4305d 100644
--- a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/decisions/__init__.py
+++ b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/decisions/__init__.py
@@ -1,7 +1,13 @@
+# Setting up runtime type checking for this package
+# We need to do this again here because the dsd imports
+# the decisions and actions from this package in a standalone way
+from beartype.claw import beartype_this_package
from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement
from bitbots_localization_handler.localization_dsd.localization_blackboard import LocalizationBlackboard
+beartype_this_package()
+
class AbstractLocalizationDecisionElement(AbstractDecisionElement):
"""
diff --git a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization_blackboard.py b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization_blackboard.py
index 3f532eca3..5e9341c92 100644
--- a/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization_blackboard.py
+++ b/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization_blackboard.py
@@ -1,3 +1,5 @@
+from typing import Optional, Type
+
import numpy as np
import tf2_ros as tf2
from bitbots_blackboard.capsules.game_status_capsule import GameStatusCapsule
@@ -5,6 +7,7 @@
from bitbots_utils.transforms import quat2euler, xyzw2wxyz
from bitbots_utils.utils import get_parameters_from_other_node
from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion
+from jaxtyping import Float
from rclpy.duration import Duration
from rclpy.node import Node
from ros2_numpy import numpify
@@ -52,18 +55,18 @@ def __init__(self, node: Node):
self.last_state_get_up = False
# IMU
- self.accel = np.array([0.0, 0.0, 0.0])
+ self.accel: Float[np.ndarray, "3"] = np.array([0.0, 0.0, 0.0])
self.imu_orientation = Quaternion(w=1.0)
# Falling odometry / imu interpolation during falls
self.imu_yaw_before_fall: float = 0.0
# Picked up
- self.pickup_accel_buffer = []
- self.pickup_accel_buffer_long = []
+ self.pickup_accel_buffer: list[Float[np.ndarray, "3"]] = []
+ self.pickup_accel_buffer_long: list[Float[np.ndarray, "3"]] = []
# Last init action
- self.last_init_action_type = False
+ self.last_init_action_type: Optional[Type] = None
self.last_init_odom_transform: TransformStamped | None = None
def _callback_pose(self, msg: PoseWithCovarianceStamped):
@@ -107,7 +110,7 @@ def picked_up(self) -> bool:
mean_long = np.mean(buffer_long[..., 2])
absolute_diff = abs(mean_long - mean)
- return absolute_diff > 1.0
+ return bool(absolute_diff > 1.0)
def get_imu_yaw(self) -> float:
"""Returns the current yaw of the IMU (this is not an absolute measurement!!! It drifts over time!)"""
diff --git a/bitbots_navigation/bitbots_localization_handler/package.xml b/bitbots_navigation/bitbots_localization_handler/package.xml
index ad35217fe..ce1b07b2f 100644
--- a/bitbots_navigation/bitbots_localization_handler/package.xml
+++ b/bitbots_navigation/bitbots_localization_handler/package.xml
@@ -27,6 +27,7 @@
std_msgs
tf2_geometry_msgs
tf2_ros
+ ament_mypy
diff --git a/bitbots_navigation/bitbots_localization_handler/test/mypy.ini b/bitbots_navigation/bitbots_localization_handler/test/mypy.ini
new file mode 100644
index 000000000..6b19e8374
--- /dev/null
+++ b/bitbots_navigation/bitbots_localization_handler/test/mypy.ini
@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True
diff --git a/bitbots_navigation/bitbots_localization_handler/test/test_mypy.py b/bitbots_navigation/bitbots_localization_handler/test/test_mypy.py
new file mode 100644
index 000000000..3dacd6040
--- /dev/null
+++ b/bitbots_navigation/bitbots_localization_handler/test/test_mypy.py
@@ -0,0 +1,10 @@
+from pathlib import Path
+
+import pytest
+from ament_mypy.main import main
+
+
+@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"
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/__init__.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/__init__.py
index e69de29bb..44b4360dd 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/__init__.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/__init__.py
@@ -0,0 +1,14 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+from rclpy.node import Node
+
+from bitbots_path_planning.path_planning_parameters import bitbots_path_planning as parameters
+
+beartype_this_package()
+
+
+class NodeWithConfig(Node):
+ def __init__(self, name: str) -> None:
+ super().__init__(name)
+ self.param_listener = parameters.ParamListener(self)
+ self.config = self.param_listener.get_params()
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py
index d5a52d760..1ff8568e9 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/controller.py
@@ -4,19 +4,20 @@
import tf2_ros as tf2
from geometry_msgs.msg import Twist
from nav_msgs.msg import Path
-from rclpy.node import Node
from rclpy.time import Time
from ros2_numpy import numpify
from tf2_geometry_msgs import PointStamped, Pose, PoseStamped
from tf_transformations import euler_from_quaternion
+from bitbots_path_planning import NodeWithConfig
+
class Controller:
"""
A simple follow the carrot controller which controls the robots command velocity to stay on a given path.
"""
- def __init__(self, node: Node, buffer: tf2.Buffer) -> None:
+ def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface) -> None:
self.node = node
self.buffer = buffer
@@ -27,7 +28,7 @@ def __init__(self, node: Node, buffer: tf2.Buffer) -> None:
self.last_update_time: Optional[Time] = None
# Accumulator for the angular error
- self.angular_error_accumulator = 0
+ self.angular_error_accumulator = 0.0
def step(self, path: Path) -> tuple[Twist, PointStamped]:
"""
@@ -52,10 +53,9 @@ def step(self, path: Path) -> tuple[Twist, PointStamped]:
# End conditions with an empty or shorter than carrot distance path need to be considered
if len(path.poses) > 0:
end_pose: Pose = path.poses[-1].pose
+ goal_pose: Pose = end_pose
if len(path.poses) > self.node.config.controller.carrot_distance:
- goal_pose: Pose = path.poses[self.node.config.controller.carrot_distance].pose
- else:
- goal_pose: Pose = end_pose
+ goal_pose = path.poses[self.node.config.controller.carrot_distance].pose
else:
return cmd_vel
@@ -91,7 +91,7 @@ def step(self, path: Path) -> tuple[Twist, PointStamped]:
# Check if we are so close to the final position of the global plan that we want to align us with
# its orientation and not the heading towards its position
- diff = 0
+ diff = 0.0
if distance > self.node.config.controller.orient_to_goal_distance:
# Calculate the difference between our current heading and the heading towards the final position of
# the global plan
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py
index 44986bb6b..9754a7659 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/map.py
@@ -5,17 +5,18 @@
from bitbots_utils.utils import get_parameters_from_other_node
from geometry_msgs.msg import Point
from nav_msgs.msg import OccupancyGrid
-from rclpy.node import Node
from ros2_numpy import msgify, numpify
from tf2_geometry_msgs import PointStamped, PoseWithCovarianceStamped
+from bitbots_path_planning import NodeWithConfig
+
class Map:
"""
Costmap that keeps track of obstacles like the ball or other robots.
"""
- def __init__(self, node: Node, buffer: tf2.Buffer) -> None:
+ def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface) -> None:
self.node = node
self.buffer = buffer
self.resolution: int = self.node.config.map.resolution
@@ -64,7 +65,7 @@ def _render_balls(self) -> None:
round(self.config_ball_diameter / 2 * self.resolution),
self.config_obstacle_value,
-1,
- )
+ ) # type: ignore
def set_robots(self, robots: sv3dm.RobotArray):
"""
@@ -95,7 +96,7 @@ def _render_robots(self) -> None:
round(max(numpify(robot.bb.size)[:2]) * self.resolution),
self.config_obstacle_value,
-1,
- )
+ ) # type: ignore
def to_map_space(self, x: float, y: float) -> tuple[int, int]:
"""
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
index 3a812d5c1..a9374c8ad 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/path_planning.py
@@ -6,24 +6,21 @@
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
from rclpy.duration import Duration
from rclpy.executors import MultiThreadedExecutor
-from rclpy.node import Node
from std_msgs.msg import Bool, Empty
+from bitbots_path_planning import NodeWithConfig
from bitbots_path_planning.controller import Controller
from bitbots_path_planning.map import Map
-from bitbots_path_planning.path_planning_parameters import bitbots_path_planning as parameters
from bitbots_path_planning.planner import planner_factory
-class PathPlanning(Node):
+class PathPlanning(NodeWithConfig):
"""
A minimal python path planning.
"""
def __init__(self) -> None:
super().__init__("bitbots_path_planning")
- self.param_listener = parameters.ParamListener(self)
- self.config = self.param_listener.get_params()
# We need to create a tf buffer
self.tf_buffer = Buffer(self, Duration(seconds=self.config.tf_buffer_duration))
@@ -61,7 +58,7 @@ def __init__(self) -> None:
self.create_subscription(
Bool,
"ball_obstacle_active",
- lambda msg: self.map.avoid_ball(msg.data),
+ lambda msg: self.map.avoid_ball(msg.data), # type: ignore
5,
callback_group=MutuallyExclusiveCallbackGroup(),
)
diff --git a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
index 08a5392cb..9b2c34e69 100644
--- a/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
+++ b/bitbots_navigation/bitbots_path_planning/bitbots_path_planning/planner.py
@@ -4,10 +4,10 @@
from geometry_msgs.msg import PoseStamped, Vector3
from nav_msgs.msg import Path
from rclpy.duration import Duration
-from rclpy.node import Node
from rclpy.time import Time
from std_msgs.msg import Header
+from bitbots_path_planning import NodeWithConfig
from bitbots_path_planning.map import Map
@@ -16,7 +16,7 @@ class Planner:
A simple grid based A* interface
"""
- def __init__(self, node: Node, buffer: tf2.Buffer, map: Map) -> None:
+ def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface, map: Map) -> None:
self.node = node
self.buffer = buffer
self.map = map
@@ -56,7 +56,8 @@ def step(self) -> Path:
"""
Generates a new A* path to the goal pose with respect to the costmap
"""
- goal = self.goal
+ goal: PoseStamped = self.goal
+ assert goal is not None, "No goal set, cannot plan path"
# Get current costmap
navigation_grid = self.map.get_map()
@@ -103,7 +104,7 @@ def get_path(self) -> Path | None:
class DummyPlanner(Planner):
- def __init__(self, node: Node, buffer: tf2.Buffer, map: Map) -> None:
+ def __init__(self, node: NodeWithConfig, buffer: tf2.BufferInterface, map: Map) -> None:
super().__init__(node, buffer, map)
def step(self) -> Path:
@@ -123,7 +124,7 @@ def get_path(self) -> Path:
return self.path
-def planner_factory(node: Node) -> type:
+def planner_factory(node: NodeWithConfig) -> type:
if node.config.planner.dummy:
return DummyPlanner
else:
diff --git a/bitbots_navigation/bitbots_path_planning/package.xml b/bitbots_navigation/bitbots_path_planning/package.xml
index 34c6ac13c..6634a76ee 100644
--- a/bitbots_navigation/bitbots_path_planning/package.xml
+++ b/bitbots_navigation/bitbots_path_planning/package.xml
@@ -24,6 +24,7 @@
ament_copyright
ament_flake8
+ ament_mypy
ament_pep257
python3-pytest
diff --git a/bitbots_navigation/bitbots_path_planning/setup.py b/bitbots_navigation/bitbots_path_planning/setup.py
index 1f987ede2..d18b72a7e 100644
--- a/bitbots_navigation/bitbots_path_planning/setup.py
+++ b/bitbots_navigation/bitbots_path_planning/setup.py
@@ -7,6 +7,7 @@
"path_planning_parameters", # python module name for parameter library
"config/path_planning_parameters.yaml", # path to input yaml file
)
+
package_name = "bitbots_path_planning"
setup(
diff --git a/bitbots_navigation/bitbots_path_planning/test/mypy.ini b/bitbots_navigation/bitbots_path_planning/test/mypy.ini
new file mode 100644
index 000000000..19bad04b6
--- /dev/null
+++ b/bitbots_navigation/bitbots_path_planning/test/mypy.ini
@@ -0,0 +1,4 @@
+# Global options:
+
+[mypy]
+ignore_missing_imports = true
diff --git a/bitbots_navigation/bitbots_path_planning/test/test_controller.py b/bitbots_navigation/bitbots_path_planning/test/test_controller.py
index b3f95930f..05276fd92 100644
--- a/bitbots_navigation/bitbots_path_planning/test/test_controller.py
+++ b/bitbots_navigation/bitbots_path_planning/test/test_controller.py
@@ -3,6 +3,7 @@
import pytest
import tf2_ros as tf2
+from bitbots_path_planning import NodeWithConfig
from bitbots_path_planning.controller import Controller
from bitbots_path_planning.path_planning_parameters import bitbots_path_planning as gen_params
from geometry_msgs.msg import Twist
@@ -131,7 +132,7 @@ def test_step_cmd_vel_smoothing(snapshot, node, tf2_buffer, config, pose_opponen
assert str(controller.last_cmd_vel) == snapshot
-def setup_controller(node: Node, buffer: tf2.Buffer) -> Controller:
+def setup_controller(node: NodeWithConfig, buffer: tf2.BufferInterface) -> Controller:
return Controller(node, buffer)
@@ -228,7 +229,7 @@ def config() -> gen_params.Params:
@pytest.fixture
def node(config) -> Node:
- node = Mock(Node)
+ node = Mock(NodeWithConfig)
node.config = config
diff --git a/bitbots_navigation/bitbots_path_planning/test/test_mypy.py b/bitbots_navigation/bitbots_path_planning/test/test_mypy.py
new file mode 100644
index 000000000..3dacd6040
--- /dev/null
+++ b/bitbots_navigation/bitbots_path_planning/test/test_mypy.py
@@ -0,0 +1,10 @@
+from pathlib import Path
+
+import pytest
+from ament_mypy.main import main
+
+
+@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"
diff --git a/bitbots_simulation/bitbots_webots_sim/CMakeLists.txt b/bitbots_simulation/bitbots_webots_sim/CMakeLists.txt
index 2562bc709..405b00503 100644
--- a/bitbots_simulation/bitbots_webots_sim/CMakeLists.txt
+++ b/bitbots_simulation/bitbots_webots_sim/CMakeLists.txt
@@ -81,6 +81,11 @@ install(
scripts/start_single.py scripts/start_webots_ros_supervisor.py
DESTINATION lib/${PROJECT_NAME})
+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 --git a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/__init__.py b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/__init__.py
+++ b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py
index 925fb1db3..a16c6e99a 100644
--- a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py
+++ b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_robot_controller.py
@@ -1,8 +1,9 @@
import math
import os
import time
+from typing import Optional
-from controller import Node, Robot
+from controller import Robot
from geometry_msgs.msg import PointStamped
from rclpy.node import Node as RclpyNode
from rclpy.time import Time
@@ -16,7 +17,7 @@
class RobotController:
def __init__(
self,
- ros_node: Node = None,
+ ros_node: Optional[RclpyNode] = None,
ros_active=False,
robot="wolfgang",
robot_node=None,
@@ -32,12 +33,15 @@ def __init__(
:param ros_active: Whether ROS messages should be published
:param robot: The name of the robot to use, currently one of wolfgang, darwin, nao, op3
- :param external_controller: Whether an external controller is used, necessary for RobotSupervisorController
+ :param robot_node: The Webots node (!= ROS node) of the robot, can normally be left empty
:param base_ns: The namespace of this node, can normally be left empty
+ :param recognize: Whether we want to store images and automatically generated ground truth labels
+ :param camera_active: Whether the camera should be active
+ :param foot_sensors_active: Whether the foot sensors should be active
"""
+ if ros_node is None:
+ ros_node = RclpyNode("robot_controller")
self.ros_node = ros_node
- if self.ros_node is None:
- self.ros_node = RclpyNode("robot_controller")
self.ros_active = ros_active
self.recognize = recognize
self.camera_active = camera_active
@@ -47,7 +51,7 @@ def __init__(
else:
self.robot_node = robot_node
self.walkready = [0] * 20
- self.time = 0
+ self.time = 0.0
self.motors = []
self.sensors = []
@@ -57,7 +61,7 @@ def __init__(
self.timestep = int(self.robot_node.getBasicTimeStep())
self.is_wolfgang = False
- self.pressure_sensors = None
+ self.pressure_sensors: Optional[list[tuple]] = None
self.pressure_sensor_names = []
if robot == "wolfgang":
self.is_wolfgang = True
@@ -618,7 +622,7 @@ def __init__(
self.camera.recognitionEnable(self.timestep)
self.last_img_saved = 0.0
self.img_save_dir = (
- "/tmp/webots/images" + time.strftime("%Y-%m-%d-%H-%M-%S") + os.getenv("WEBOTS_ROBOT_NAME")
+ "/tmp/webots/images" + time.strftime("%Y-%m-%d-%H-%M-%S") + os.getenv("WEBOTS_ROBOT_NAME", "")
)
if not os.path.exists(self.img_save_dir):
os.makedirs(self.img_save_dir)
@@ -925,17 +929,17 @@ def get_pressure_message(self):
left_pressure = FootPressure()
left_pressure.header.stamp = current_time
- left_pressure.left_back = self.pressure_sensors[0].getValue()
- left_pressure.left_front = self.pressure_sensors[1].getValue()
- left_pressure.right_front = self.pressure_sensors[2].getValue()
- left_pressure.right_back = self.pressure_sensors[3].getValue()
+ left_pressure.left_back = self.pressure_sensors[0].getValue() # type: ignore
+ left_pressure.left_front = self.pressure_sensors[1].getValue() # type: ignore
+ left_pressure.right_front = self.pressure_sensors[2].getValue() # type: ignore
+ left_pressure.right_back = self.pressure_sensors[3].getValue() # type: ignore
right_pressure = FootPressure()
right_pressure.header.stamp = current_time
- right_pressure.left_back = self.pressure_sensors[4].getValue()
- right_pressure.left_front = self.pressure_sensors[5].getValue()
- right_pressure.right_front = self.pressure_sensors[6].getValue()
- right_pressure.right_back = self.pressure_sensors[7].getValue()
+ right_pressure.left_back = self.pressure_sensors[4].getValue() # type: ignore
+ right_pressure.left_front = self.pressure_sensors[5].getValue() # type: ignore
+ right_pressure.right_front = self.pressure_sensors[6].getValue() # type: ignore
+ right_pressure.right_back = self.pressure_sensors[7].getValue() # type: ignore
# compute center of pressures of the feet
pos_x = 0.085
diff --git a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py
index e2b51cb70..cce9a575f 100644
--- a/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py
+++ b/bitbots_simulation/bitbots_webots_sim/bitbots_webots_sim/webots_supervisor_controller.py
@@ -1,3 +1,5 @@
+from typing import Optional
+
import numpy as np
import transforms3d
from controller import Keyboard, Node, Supervisor
@@ -16,7 +18,7 @@
class SupervisorController:
def __init__(
self,
- ros_node: Node = None,
+ ros_node: Optional[RclpyNode] = None,
ros_active=False,
mode="normal",
base_ns="/",
@@ -31,13 +33,13 @@ def __init__(
:param mode: Webots mode, one of 'normal', 'paused', or 'fast'
:param base_ns: The namespace of this node, can normally be left empty
"""
+ if ros_node is None:
+ ros_node = RclpyNode("supervisor_controller")
self.ros_node = ros_node
- if self.ros_node is None:
- self.ros_node = RclpyNode("supervisor_controller")
# requires WEBOTS_ROBOT_NAME to be set to "supervisor_robot"
self.ros_active = ros_active
self.model_states_active = model_states_active
- self.time = 0
+ self.time = 0.0
self.clock_msg = Clock()
self.supervisor = Supervisor()
self.keyboard_activated = False
@@ -51,8 +53,8 @@ def __init__(
else:
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_REAL_TIME)
- self.motors = []
- self.sensors = []
+ self.motors: list = []
+ self.sensors: list = []
self.timestep = int(self.supervisor.getBasicTimeStep())
self.robot_nodes = {}
@@ -75,7 +77,7 @@ def __init__(
}
if self.robot_type not in reset_heights.keys():
self.ros_node.get_logger().warn(f"Robot type {self.robot_type} has no reset height defined. Will use 1m")
- self.reset_height = 1
+ self.reset_height = 1.0
else:
self.reset_height = reset_heights[self.robot_type]
diff --git a/bitbots_simulation/bitbots_webots_sim/mypy.ini b/bitbots_simulation/bitbots_webots_sim/mypy.ini
new file mode 100644
index 000000000..6b19e8374
--- /dev/null
+++ b/bitbots_simulation/bitbots_webots_sim/mypy.ini
@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True
diff --git a/bitbots_simulation/bitbots_webots_sim/package.xml b/bitbots_simulation/bitbots_webots_sim/package.xml
index dc368db75..5c01aae18 100644
--- a/bitbots_simulation/bitbots_webots_sim/package.xml
+++ b/bitbots_simulation/bitbots_webots_sim/package.xml
@@ -27,7 +27,7 @@
python3-numpy
python3-psutil
std_srvs
-
+ ament_cmake_mypy
imu_complementary_filter
xvfb
diff --git a/bitbots_team_communication/bitbots_team_communication/CMakeLists.txt b/bitbots_team_communication/bitbots_team_communication/CMakeLists.txt
index d6965a5bd..35bd6de10 100644
--- a/bitbots_team_communication/bitbots_team_communication/CMakeLists.txt
+++ b/bitbots_team_communication/bitbots_team_communication/CMakeLists.txt
@@ -57,6 +57,9 @@ if(BUILD_TESTING)
WORKING_DIRECTORY
"${CMAKE_SOURCE_DIR}")
endforeach()
+
+ find_package(ament_cmake_mypy REQUIRED)
+ ament_mypy(CONFIG_FILE "${CMAKE_CURRENT_LIST_DIR}/mypy.ini")
endif()
ament_package()
diff --git a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/__init__.py b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/__init__.py
+++ b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py
index 0670944fe..79675e5a6 100755
--- a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py
+++ b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py
@@ -9,6 +9,7 @@
from ament_index_python.packages import get_package_share_directory
from bitbots_tf_buffer import Buffer
from bitbots_utils.utils import get_parameter_dict, get_parameters_from_other_node
+from builtin_interfaces.msg import Time as TimeMsg
from game_controller_hl_interfaces.msg import GameState
from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, TwistWithCovarianceStamped
from numpy import double
@@ -83,7 +84,7 @@ def set_state_defaults(self):
self.cmd_vel: Optional[Twist] = None
self.cmd_vel_time = Time(clock_type=self.node.get_clock().clock_type)
self.ball: Optional[PointStamped] = None
- self.ball_velocity: Tuple[float, float, float] = (0, 0, 0)
+ self.ball_velocity: Tuple[float, float, float] = (0.0, 0.0, 0.0)
self.ball_covariance: List[double] = []
self.strategy: Optional[Strategy] = None
self.strategy_time = Time(clock_type=self.node.get_clock().clock_type)
@@ -188,7 +189,7 @@ def move_base_goal_cb(self, msg: PoseStamped):
self.move_base_goal = msg
def robots_cb(self, msg: RobotArray):
- def transform_to_map(robot_relative: Robot):
+ def transform_to_map(robot_relative: Robot) -> Optional[Robot]:
# @TODO: check if this is not handled by the transform itself
robot_pose = PoseStamped(header=msg.header, pose=robot_relative.bb.center)
try:
@@ -197,8 +198,9 @@ def transform_to_map(robot_relative: Robot):
return robot_relative
except TransformException as err:
self.logger.error(f"Could not transform robot to map frame: {err}")
+ return None
- robots_on_map = list(filter(None, map(transform_to_map, msg.robots)))
+ robots_on_map: list[Robot] = list(filter(None, map(transform_to_map, msg.robots)))
self.seen_robots = RobotArray(header=msg.header, robots=robots_on_map)
self.seen_robots.header.frame_id = self.map_frame
@@ -252,8 +254,8 @@ def send_message(self):
now = self.get_current_time()
msg = self.create_empty_message(now)
- def is_still_valid(time) -> bool:
- return now - Time.from_msg(time) < Duration(seconds=self.lifetime)
+ def is_still_valid(time: Optional[TimeMsg]) -> bool:
+ return (time is not None) and (now - Time.from_msg(time) < Duration(seconds=self.lifetime))
message = self.protocol_converter.convert_to_message(self, msg, is_still_valid)
self.socket_communication.send_message(message.SerializeToString())
diff --git a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/communication.py b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/communication.py
index 14d8966c5..e9ea7c535 100644
--- a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/communication.py
+++ b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/communication.py
@@ -43,12 +43,12 @@ def get_connection(self) -> socket.socket:
def close_connection(self):
if self.is_setup():
- self.socket.close()
+ self.socket.close() # type: ignore[union-attr]
self.logger.info("Connection closed.")
def receive_message(self) -> Optional[bytes]:
self.assert_is_setup()
- msg, _, flags, _ = self.socket.recvmsg(self.buffer_size)
+ msg, _, flags, _ = self.socket.recvmsg(self.buffer_size) # type: ignore[union-attr]
is_message_truncated = flags & socket.MSG_TRUNC
if is_message_truncated:
self.handle_truncated_message()
@@ -67,7 +67,7 @@ def send_message(self, message):
self.assert_is_setup()
for port in self.target_ports:
self.logger.debug(f"Sending message to {port} on {self.target_ip}")
- self.socket.sendto(message, (str(self.target_ip), port))
+ self.socket.sendto(message, (str(self.target_ip), port)) # type: ignore[union-attr]
def assert_is_setup(self):
assert self.is_setup(), "Socket is not yet initialized"
diff --git a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/message_to_team_data_converter.py b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/message_to_team_data_converter.py
index 1dd4b0fe2..5f5d2fe62 100644
--- a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/message_to_team_data_converter.py
+++ b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/message_to_team_data_converter.py
@@ -1,8 +1,9 @@
-from typing import List, Tuple
+from typing import Iterable, Sequence, Tuple
+import numpy as np
import transforms3d
from geometry_msgs.msg import PoseWithCovariance
-from numpy import double
+from jaxtyping import Float64
import bitbots_team_communication.robocup_extension_pb2 as Proto # noqa: N812
from bitbots_msgs.msg import RobotRelative, RobotRelativeArray, TeamData
@@ -44,7 +45,7 @@ def convert_strategy(self, message: Proto.Message, team_data: TeamData) -> TeamD
return team_data
def convert_robots(
- self, message_robots: List[Proto.Robot], message_robot_confidence: List[float]
+ self, message_robots: Iterable[Proto.Robot], message_robot_confidence: Sequence[float]
) -> RobotRelativeArray:
relative_robots = RobotRelativeArray()
for index, robot in enumerate(message_robots):
@@ -74,7 +75,7 @@ def convert_robot_pose(self, message_robot_pose: Proto.Robot) -> PoseWithCovaria
robot.pose.position.x = message_robot_pose.position.x
robot.pose.position.y = message_robot_pose.position.y
- quaternion = self.convert_to_quat((0, 0, message_robot_pose.position.z))
+ quaternion = self.convert_to_quat((0.0, 0.0, message_robot_pose.position.z))
robot.pose.orientation.w = quaternion[0]
robot.pose.orientation.x = quaternion[1]
robot.pose.orientation.y = quaternion[2]
@@ -87,7 +88,9 @@ def convert_robot_pose(self, message_robot_pose: Proto.Robot) -> PoseWithCovaria
def convert_to_quat(self, euler_angles: Tuple[float, float, float]):
return transforms3d.euler.euler2quat(*euler_angles)
- def convert_to_row_major_covariance(self, row_major_covariance: List[double], covariance_matrix: Proto.fmat3):
+ def convert_to_row_major_covariance(
+ self, row_major_covariance: Float64[np.ndarray, "36"], covariance_matrix: Proto.fmat3
+ ):
# ROS covariance is row-major 36 x float, while protobuf covariance
# is column-major 9 x float [x, y, θ]
row_major_covariance[0] = covariance_matrix.x.x
diff --git a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/state_to_message_converter.py b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/state_to_message_converter.py
index 056c73dd7..e913d27df 100644
--- a/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/state_to_message_converter.py
+++ b/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/state_to_message_converter.py
@@ -1,11 +1,12 @@
import math
-from typing import Callable, List, Optional, Tuple
+from typing import Callable, Optional, Tuple
+import numpy as np
import transforms3d
+from builtin_interfaces.msg import Time
from game_controller_hl_interfaces.msg import GameState
from geometry_msgs.msg import PointStamped, PoseStamped, PoseWithCovarianceStamped, Quaternion, Twist
-from numpy import double
-from rclpy.time import Time
+from jaxtyping import Float64
from soccer_vision_3d_msgs.msg import Robot, RobotArray
import bitbots_team_communication.robocup_extension_pb2 as Proto # noqa: N812
@@ -19,7 +20,9 @@ def __init__(self, team_mapping, role_mapping, action_mapping, side_mapping):
self.action_mapping = action_mapping
self.side_mapping = side_mapping
- def convert(self, state, message: Proto.Message, is_still_valid_checker: Callable[[Time], bool]) -> Proto.Message:
+ def convert(
+ self, state, message: Proto.Message, is_still_valid_checker: Callable[[Optional[Time]], bool]
+ ) -> Proto.Message:
def convert_gamestate(gamestate: Optional[GameState], message: Proto.Message):
if gamestate is not None and is_still_valid_checker(gamestate.header.stamp):
message.state = Proto.State.PENALISED if gamestate.penalized else Proto.State.UNPENALISED
@@ -43,7 +46,9 @@ def convert_current_pose(current_pose: Optional[PoseWithCovarianceStamped], mess
return message
- def convert_walk_command(walk_command: Optional[Twist], walk_command_time: Time, message: Proto.Message):
+ def convert_walk_command(
+ walk_command: Optional[Twist], walk_command_time: Optional[Time], message: Proto.Message
+ ):
if walk_command is not None and is_still_valid_checker(walk_command_time):
message.walk_command.x = walk_command.linear.x
message.walk_command.y = walk_command.linear.y
@@ -63,7 +68,7 @@ def convert_target_position(target_position: Optional[PoseStamped], message):
def convert_ball_position(
ball_position: Optional[PointStamped],
ball_velocity: Tuple[float, float, float],
- ball_covariance: List[double],
+ ball_covariance: Float64[np.ndarray, "36"],
message,
):
if ball_position is not None and is_still_valid_checker(ball_position.header.stamp):
@@ -99,7 +104,7 @@ def convert_seen_robots(seen_robots: Optional[RobotArray], message: Proto.Messag
return message
- def convert_strategy(strategy: Optional[Strategy], strategy_time: Time, message: Proto.Message):
+ def convert_strategy(strategy: Optional[Strategy], strategy_time: Optional[Time], message: Proto.Message):
if strategy is not None and is_still_valid_checker(strategy_time):
message.role = self.role_mapping[strategy.role]
message.action = self.action_mapping[strategy.action]
@@ -129,9 +134,9 @@ def calculate_time_to_ball(
def convert_time_to_ball(
time_to_ball: Optional[float],
- time_to_ball_time: Time,
- ball_position: PointStamped,
- current_pose: PoseWithCovarianceStamped,
+ time_to_ball_time: Optional[Time],
+ ball_position: Optional[PointStamped],
+ current_pose: Optional[PoseWithCovarianceStamped],
walking_speed: float,
message: Proto.Message,
):
@@ -147,16 +152,24 @@ def convert_time_to_ball(
message.current_pose.player_id = state.player_id
message.current_pose.team = state.team_id
- message = convert_gamestate(state.gamestate, message)
- message = convert_current_pose(state.pose, message)
- message = convert_walk_command(state.cmd_vel, state.cmd_vel_time, message)
- message = convert_target_position(state.move_base_goal, message)
- message = convert_ball_position(state.ball, state.ball_velocity, state.ball_covariance, message)
- message = convert_seen_robots(state.seen_robots, message)
- message = convert_strategy(state.strategy, state.strategy_time, message)
- message = convert_time_to_ball(
- state.time_to_ball, state.time_to_ball_time, state.ball, state.pose, state.avg_walking_speed, message
- )
+ if state.gamestate is not None:
+ message = convert_gamestate(state.gamestate, message)
+ if state.pose is not None:
+ message = convert_current_pose(state.pose, message)
+ if state.cmd_vel is not None:
+ message = convert_walk_command(state.cmd_vel, state.cmd_vel_time, message)
+ if convert_target_position is not None:
+ message = convert_target_position(state.move_base_goal, message)
+ if state.ball is not None:
+ message = convert_ball_position(state.ball, state.ball_velocity, state.ball_covariance, message)
+ if state.seen_robots is not None:
+ message = convert_seen_robots(state.seen_robots, message)
+ if state.strategy is not None:
+ message = convert_strategy(state.strategy, state.strategy_time, message)
+ if state.time_to_ball is not None:
+ message = convert_time_to_ball(
+ state.time_to_ball, state.time_to_ball_time, state.ball, state.pose, state.avg_walking_speed, message
+ )
return message
@@ -174,7 +187,9 @@ def extract_orientation_yaw_angle(self, quaternion: Quaternion):
def convert_to_euler(self, quaternion: Quaternion):
return transforms3d.euler.quat2euler([quaternion.w, quaternion.x, quaternion.y, quaternion.z])
- def convert_to_covariance_matrix(self, covariance_matrix: Proto.fmat3, row_major_covariance: List[double]):
+ def convert_to_covariance_matrix(
+ self, covariance_matrix: Proto.fmat3, row_major_covariance: Float64[np.ndarray, "36"]
+ ):
# ROS covariance is row-major 36 x float, while protobuf covariance
# is column-major 9 x float [x, y, θ]
covariance_matrix.x.x = row_major_covariance[0]
diff --git a/bitbots_team_communication/bitbots_team_communication/mypy.ini b/bitbots_team_communication/bitbots_team_communication/mypy.ini
new file mode 100644
index 000000000..6b19e8374
--- /dev/null
+++ b/bitbots_team_communication/bitbots_team_communication/mypy.ini
@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True
diff --git a/bitbots_team_communication/bitbots_team_communication/package.xml b/bitbots_team_communication/bitbots_team_communication/package.xml
index 34f06874b..12a0701b1 100644
--- a/bitbots_team_communication/bitbots_team_communication/package.xml
+++ b/bitbots_team_communication/bitbots_team_communication/package.xml
@@ -22,6 +22,7 @@
bitbots_msgs
bitbots_tf_buffer
bitbots_utils
+ builtin_interfaces
game_controller_hl_interfaces
geometry_msgs
python3-protobuf
@@ -31,6 +32,7 @@
tf2_geometry_msgs
tf2_ros
tf2
+ ament_cmake_mypy
python3-pytest
diff --git a/bitbots_team_communication/bitbots_team_communication/scripts/team_comm_test_marker.py b/bitbots_team_communication/bitbots_team_communication/scripts/team_comm_test_marker.py
index 5461596f9..a95ba6d2e 100755
--- a/bitbots_team_communication/bitbots_team_communication/scripts/team_comm_test_marker.py
+++ b/bitbots_team_communication/bitbots_team_communication/scripts/team_comm_test_marker.py
@@ -1,4 +1,5 @@
#!/usr/bin/env python3
+# type: ignore
import copy
import math
@@ -23,6 +24,9 @@
class TeamCommMarker:
+ marker_name: str
+ interaction_mode: int
+
def __init__(self, server):
self.server = server
self.pose = Pose()
diff --git a/bitbots_team_communication/bitbots_team_communication/test/converter/__snapshots__/test_state_to_message_converter.ambr b/bitbots_team_communication/bitbots_team_communication/test/converter/__snapshots__/test_state_to_message_converter.ambr
index 12cf92053..0e54919e2 100644
--- a/bitbots_team_communication/bitbots_team_communication/test/converter/__snapshots__/test_state_to_message_converter.ambr
+++ b/bitbots_team_communication/bitbots_team_communication/test/converter/__snapshots__/test_state_to_message_converter.ambr
@@ -63,33 +63,8 @@
'''
current_pose {
player_id: 2
- covariance {
- x {
- x: 100.0
- }
- y {
- y: 100.0
- }
- z {
- z: 100.0
- }
- }
team: BLUE
}
- ball {
- covariance {
- x {
- x: 100.0
- }
- y {
- y: 100.0
- }
- z {
- z: 100.0
- }
- }
- }
- time_to_ball: 9999.0
'''
# ---
@@ -136,5 +111,5 @@
'''
# ---
# name: test_convert_time_to_ball_calculated
- 1.2856487035751343
+ 0.0
# ---
diff --git a/bitbots_team_communication/bitbots_team_communication/test/converter/test_robocup_protocol_converter.py b/bitbots_team_communication/bitbots_team_communication/test/converter/test_robocup_protocol_converter.py
index b71aa41d6..c776238f4 100644
--- a/bitbots_team_communication/bitbots_team_communication/test/converter/test_robocup_protocol_converter.py
+++ b/bitbots_team_communication/bitbots_team_communication/test/converter/test_robocup_protocol_converter.py
@@ -1,3 +1,5 @@
+# type: ignore
+
from unittest.mock import MagicMock
import pytest
diff --git a/bitbots_team_communication/bitbots_team_communication/test/converter/test_state_to_message_converter.py b/bitbots_team_communication/bitbots_team_communication/test/converter/test_state_to_message_converter.py
index 5fd888a4d..b3513245a 100644
--- a/bitbots_team_communication/bitbots_team_communication/test/converter/test_state_to_message_converter.py
+++ b/bitbots_team_communication/bitbots_team_communication/test/converter/test_state_to_message_converter.py
@@ -2,6 +2,7 @@
import numpy
import pytest
+from builtin_interfaces.msg import Time
from game_controller_hl_interfaces.msg import GameState
from geometry_msgs.msg import (
Point,
@@ -14,7 +15,6 @@
Twist,
Vector3,
)
-from rclpy.time import Time
from soccer_vision_3d_msgs.msg import Robot, RobotArray
from soccer_vision_attribute_msgs.msg import Confidence
from soccer_vision_attribute_msgs.msg import Robot as RobotAttributes
diff --git a/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/__init__.py b/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/__init__.py
+++ b/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/team_data_ui.py b/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/team_data_ui.py
index 64dacec4b..80da523f2 100755
--- a/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/team_data_ui.py
+++ b/bitbots_team_communication/bitbots_team_data_sim_rqt/bitbots_team_data_sim_rqt/team_data_ui.py
@@ -50,7 +50,7 @@ def __init__(self, parent=None):
self.publish_button = PublishButton()
self.main_layout.addWidget(self.publish_button)
- self.id_spin_box.valueChanged.connect(self.id_spin_box_update)
+ self.id_spin_box.valueChanged.connect(self.id_spin_box_update) # type: ignore[attr-defined]
def id_spin_box_update(self):
enabled = self.id_spin_box.value() != 0
@@ -110,12 +110,12 @@ def __init__(self, parent=None):
self.time_spin_box.setRange(0, 200)
self.main_layout.addWidget(self.time_spin_box)
# Create slider
- self.time_slider = QSlider(Qt.Horizontal)
+ self.time_slider = QSlider(Qt.Horizontal) # type: ignore[attr-defined]
self.time_slider.setRange(0, 200)
self.main_layout.addWidget(self.time_slider)
# Connect spin box and slider
- self.time_spin_box.valueChanged.connect(self.time_slider.setValue)
- self.time_slider.valueChanged.connect(self.time_spin_box.setValue)
+ self.time_spin_box.valueChanged.connect(self.time_slider.setValue) # type: ignore[attr-defined]
+ self.time_slider.valueChanged.connect(self.time_spin_box.setValue) # type: ignore[attr-defined]
def get_time(self) -> float:
return float(self.time_spin_box.value())
@@ -178,7 +178,7 @@ def __init__(self, parent=None):
self.setCheckable(True)
self.setEnabled(False)
- self.clicked.connect(self.publish_button_clicked)
+ self.clicked.connect(self.publish_button_clicked) # type: ignore[attr-defined]
def publish_button_clicked(self):
if self.isChecked():
@@ -203,9 +203,6 @@ def __init__(self, context):
# Create publishers
self.team_data_pub = self._node.create_publisher(TeamData, "/team_data", 1)
- # Initialize the window
- self._widget = QMainWindow()
-
# Load XML ui definition
ui_file = os.path.join(
get_package_share_directory("bitbots_team_data_sim_rqt"), "resource", "RobotTeamDataSimulator.ui"
diff --git a/bitbots_team_communication/bitbots_team_data_sim_rqt/package.xml b/bitbots_team_communication/bitbots_team_data_sim_rqt/package.xml
index f62e2a73e..10982eb69 100644
--- a/bitbots_team_communication/bitbots_team_data_sim_rqt/package.xml
+++ b/bitbots_team_communication/bitbots_team_data_sim_rqt/package.xml
@@ -21,6 +21,7 @@
rqt_gui_py
rqt_gui
std_srvs
+ ament_mypy
diff --git a/bitbots_team_communication/bitbots_team_data_sim_rqt/setup.py b/bitbots_team_communication/bitbots_team_data_sim_rqt/setup.py
index a0f3e44aa..c99f36617 100644
--- a/bitbots_team_communication/bitbots_team_data_sim_rqt/setup.py
+++ b/bitbots_team_communication/bitbots_team_data_sim_rqt/setup.py
@@ -13,6 +13,7 @@
],
install_requires=["setuptools"],
zip_safe=True,
+ tests_require=["pytest"],
entry_points={
"console_scripts": [
"team_data_sim_gui = " + package_name + ".team_data_ui:main",
diff --git a/bitbots_team_communication/bitbots_team_data_sim_rqt/test/mypy.ini b/bitbots_team_communication/bitbots_team_data_sim_rqt/test/mypy.ini
new file mode 100644
index 000000000..6b19e8374
--- /dev/null
+++ b/bitbots_team_communication/bitbots_team_data_sim_rqt/test/mypy.ini
@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True
diff --git a/bitbots_team_communication/bitbots_team_data_sim_rqt/test/test_mypy.py b/bitbots_team_communication/bitbots_team_data_sim_rqt/test/test_mypy.py
new file mode 100644
index 000000000..3dacd6040
--- /dev/null
+++ b/bitbots_team_communication/bitbots_team_data_sim_rqt/test/test_mypy.py
@@ -0,0 +1,10 @@
+from pathlib import Path
+
+import pytest
+from ament_mypy.main import main
+
+
+@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"
diff --git a/bitbots_vision/bitbots_vision/__init__.py b/bitbots_vision/bitbots_vision/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_vision/bitbots_vision/__init__.py
+++ b/bitbots_vision/bitbots_vision/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_vision/bitbots_vision/vision_modules/candidate.py b/bitbots_vision/bitbots_vision/vision_modules/candidate.py
index 1ccbbe92d..e261a2665 100644
--- a/bitbots_vision/bitbots_vision/vision_modules/candidate.py
+++ b/bitbots_vision/bitbots_vision/vision_modules/candidate.py
@@ -127,8 +127,7 @@ def get_lower_right_y(self):
"""
return self._y1 + self._height
- def get_lower_center_point(self):
- # type: () -> (int, int)
+ def get_lower_center_point(self) -> tuple[int, int]:
"""
:return tuple: Returns the lowest point of the candidate. The point is horizontally centered inside the candidate.
"""
diff --git a/bitbots_vision/bitbots_vision/vision_modules/debug.py b/bitbots_vision/bitbots_vision/vision_modules/debug.py
index 8cd3d3208..df97333a7 100644
--- a/bitbots_vision/bitbots_vision/vision_modules/debug.py
+++ b/bitbots_vision/bitbots_vision/vision_modules/debug.py
@@ -1,5 +1,10 @@
+from typing import Callable, Optional, Sequence
+
import cv2
import numpy as np
+from jaxtyping import UInt8
+
+from bitbots_vision.vision_modules.candidate import Candidate
class DebugImage:
@@ -12,14 +17,14 @@ class DebugImage:
different obstacles (black: unknown, red: red robot, blue: blue robot).
"""
- def __init__(self, active=True):
+ def __init__(self, active: bool = True):
"""
Initialization of :class:`.DebugImage`.
"""
- self._debug_image = None
+ self._debug_image: Optional[UInt8[np.ndarray, "h w 3"]] = None
self.active = active
- def set_image(self, image):
+ def set_image(self, image: UInt8[np.ndarray, "h w 3"]) -> None:
"""
Sets a new image on which the debug image is mapped.
@@ -27,7 +32,9 @@ def set_image(self, image):
"""
self._debug_image = image.copy()
- def draw_field_boundary(self, field_boundary_points, color, thickness=1):
+ def draw_field_boundary(
+ self, field_boundary_points: Sequence[int], color: tuple[int, int, int], thickness: int = 1
+ ):
"""
Draws a line a line that represents the given field_boundary.
@@ -37,10 +44,13 @@ def draw_field_boundary(self, field_boundary_points, color, thickness=1):
"""
if not self.active:
return
+ assert self._debug_image is not None, "No image set"
for i in range(len(field_boundary_points) - 1):
- cv2.line(self._debug_image, field_boundary_points[i], field_boundary_points[i + 1], color, thickness=1)
+ cv2.line(self._debug_image, field_boundary_points[i], field_boundary_points[i + 1], color, thickness=1) # type: ignore[call-overload]
- def draw_ball_candidates(self, ball_candidates, color, thickness=1):
+ def draw_ball_candidates(
+ self, ball_candidates: Sequence[Candidate], color: tuple[int, int, int], thickness: int = 1
+ ):
"""
Draws a circle around every coordinate where a ball candidate was found.
@@ -50,6 +60,7 @@ def draw_ball_candidates(self, ball_candidates, color, thickness=1):
"""
if not self.active:
return
+ assert self._debug_image is not None, "No image set"
for candidate in ball_candidates:
if candidate:
cv2.circle(
@@ -60,7 +71,9 @@ def draw_ball_candidates(self, ball_candidates, color, thickness=1):
thickness=thickness,
)
- def draw_robot_candidates(self, robot_candidates, color, thickness=1):
+ def draw_robot_candidates(
+ self, robot_candidates: Sequence[Candidate], color: tuple[int, int, int], thickness: int = 1
+ ):
"""
Draws a bounding box for every given robot.
@@ -70,17 +83,17 @@ def draw_robot_candidates(self, robot_candidates, color, thickness=1):
"""
if not self.active:
return
+ assert self._debug_image is not None, "No image set"
for candidate in robot_candidates:
- if candidate:
- cv2.rectangle(
- self._debug_image,
- candidate.get_upper_left_point(),
- candidate.get_lower_right_point(),
- color,
- thickness=thickness,
- )
+ cv2.rectangle(
+ self._debug_image,
+ candidate.get_upper_left_point(),
+ candidate.get_lower_right_point(),
+ color,
+ thickness=thickness,
+ )
- def draw_points(self, points, color, thickness=-1, rad=2):
+ def draw_points(self, points: tuple[int, int], color: tuple[int, int, int], thickness: int = -1, rad: int = 2):
"""
Draws a (line)point for every given point.
@@ -91,10 +104,13 @@ def draw_points(self, points, color, thickness=-1, rad=2):
"""
if not self.active:
return
+ assert self._debug_image is not None, "No image set"
for point in points:
- cv2.circle(self._debug_image, point, rad, color, thickness=thickness)
+ cv2.circle(self._debug_image, point, rad, color, thickness=thickness) # type: ignore[call-overload]
- def draw_line_segments(self, segments, color, thickness=2):
+ def draw_line_segments(
+ self, segments: Sequence[tuple[int, int, int, int]], color: tuple[int, int, int], thickness: int = 2
+ ):
"""
Draws a line segment.
@@ -104,12 +120,14 @@ def draw_line_segments(self, segments, color, thickness=2):
"""
if not self.active:
return
+ assert self._debug_image is not None, "No image set"
for segment in segments:
cv2.line(self._debug_image, (segment[0], segment[1]), (segment[2], segment[3]), color, thickness=2)
def draw_mask(self, mask, color, opacity=0.5):
if not self.active:
return
+ assert self._debug_image is not None, "No image set"
# Make a colored image
colored_image = np.zeros_like(self._debug_image)
colored_image[:, :] = tuple(np.multiply(color, opacity).astype(np.uint8))
@@ -150,7 +168,7 @@ def draw(self, debug_image_description, image=None):
if image:
self.set_image(image)
# Define the draw functions for each type
- draw_functions = {
+ draw_functions: dict[str, Callable] = {
"robot": self.draw_robot_candidates,
"field_boundary": self.draw_field_boundary,
"ball": self.draw_ball_candidates,
diff --git a/bitbots_vision/bitbots_vision/vision_modules/ros_utils.py b/bitbots_vision/bitbots_vision/vision_modules/ros_utils.py
index 49eec47c1..84cc14da8 100644
--- a/bitbots_vision/bitbots_vision/vision_modules/ros_utils.py
+++ b/bitbots_vision/bitbots_vision/vision_modules/ros_utils.py
@@ -1,5 +1,5 @@
import re
-from typing import Union
+from typing import Optional, TypeAlias
from bitbots_utils.utils import get_parameters_from_other_node
from cv_bridge import CvBridge
@@ -32,6 +32,8 @@
global own_team_color
own_team_color = None
+T_RobotAttributes_Team: TypeAlias = int # Type for RobotAttributes.team
+
def create_or_update_publisher(
node, old_config, new_config, publisher_object, topic_key, data_class, qos_profile=1, callback_group=None
@@ -325,7 +327,7 @@ def update_own_team_color(vision_node: Node):
vision_node._logger.debug(f"Own team color is: {own_team_color}")
-def get_team_from_robot_color(color: int) -> RobotAttributes.team:
+def get_team_from_robot_color(color: int) -> T_RobotAttributes_Team:
"""
Maps the detected robot color to the current team.
If the color is the same as the current team, returns own team, else returns opponent team.
@@ -343,7 +345,7 @@ def get_team_from_robot_color(color: int) -> RobotAttributes.team:
return RobotAttributes.TEAM_OPPONENT
-def get_robot_color_for_team(team: RobotAttributes.team) -> Union[int, None]:
+def get_robot_color_for_team(team: T_RobotAttributes_Team) -> Optional[int]:
"""
Maps team (own, opponent, unknown) to the current robot color.
"""
@@ -358,3 +360,5 @@ def get_robot_color_for_team(team: RobotAttributes.team) -> Union[int, None]:
return 1
else:
return 0
+ else:
+ return None
diff --git a/bitbots_vision/bitbots_vision/vision_modules/yoeo/model_config.py b/bitbots_vision/bitbots_vision/vision_modules/yoeo/model_config.py
index 975217433..32a1f6fbe 100644
--- a/bitbots_vision/bitbots_vision/vision_modules/yoeo/model_config.py
+++ b/bitbots_vision/bitbots_vision/vision_modules/yoeo/model_config.py
@@ -1,7 +1,7 @@
from os.path import join
from typing import Dict, List, Optional
-import yaml
+import yaml # type: ignore[import-untyped]
class ModelConfig:
diff --git a/bitbots_vision/bitbots_vision/vision_modules/yoeo/object_manager.py b/bitbots_vision/bitbots_vision/vision_modules/yoeo/object_manager.py
index b631a622c..ea76a1026 100644
--- a/bitbots_vision/bitbots_vision/vision_modules/yoeo/object_manager.py
+++ b/bitbots_vision/bitbots_vision/vision_modules/yoeo/object_manager.py
@@ -1,5 +1,5 @@
import os.path as osp
-from typing import Dict, Optional
+from typing import Dict, Optional, Type
import rclpy
@@ -16,7 +16,7 @@ class YOEOObjectManager:
This class manages the creation and update of the YOEO handler instance.
"""
- _HANDLERS_BY_NAME = {
+ _HANDLERS_BY_NAME: dict[str, Type[yoeo_handlers.YOEOHandlerTemplate]] = {
"openvino": yoeo_handlers.YOEOHandlerOpenVino,
"onnx": yoeo_handlers.YOEOHandlerONNX,
"pytorch": yoeo_handlers.YOEOHandlerPytorch,
@@ -47,10 +47,8 @@ def get(cls) -> yoeo_handlers.IYOEOHandler:
:return: the current YOEO handler instance
:rtype: IYOEOHandler
"""
- if cls._yoeo_instance is None:
- logger.error("No yoeo handler created yet!")
- else:
- return cls._yoeo_instance
+ assert cls._yoeo_instance is not None, "YOEO handler instance not set!"
+ return cls._yoeo_instance
@classmethod
def get_id(cls) -> int:
@@ -60,10 +58,8 @@ def get_id(cls) -> int:
:return: the ID of the current YOEO handler instance
:rtype: int
"""
- if cls._yoeo_instance is None:
- logger.error("No yoeo handler created yet")
- else:
- return id(cls._yoeo_instance)
+ assert cls._yoeo_instance is not None, "YOEO handler instance not set!"
+ return id(cls._yoeo_instance)
@classmethod
def is_team_color_detection_supported(cls) -> bool:
@@ -116,6 +112,7 @@ def _configure_yoeo_instance(cls, config: Dict, framework: str, model_path: str)
cls._load_model_config(model_path)
cls._instantiate_new_yoeo_handler(config, framework, model_path)
elif cls._yoeo_parameters_have_changed(config):
+ assert cls._yoeo_instance is not None, "YOEO handler instance not set!"
cls._yoeo_instance.configure(config)
@classmethod
diff --git a/bitbots_vision/bitbots_vision/vision_modules/yoeo/utils.py b/bitbots_vision/bitbots_vision/vision_modules/yoeo/utils.py
index f636f5d5b..a1a4a4c4d 100644
--- a/bitbots_vision/bitbots_vision/vision_modules/yoeo/utils.py
+++ b/bitbots_vision/bitbots_vision/vision_modules/yoeo/utils.py
@@ -5,6 +5,7 @@
import cv2
import numpy as np
import rclpy
+from jaxtyping import UInt8
logger = rclpy.logging.get_logger("yoeo_handler_utils")
@@ -178,8 +179,8 @@ def get_info(self) -> ImagePreProcessorData:
max_dim=np.max(self._image_dimensions_HW),
)
- def process(self, image: np.ndarray) -> np.ndarray:
- self._image_dimensions_HW = image.shape[:2]
+ def process(self, image: UInt8[np.ndarray, "h w 3"]) -> UInt8[np.ndarray, "3 network_input_h network_input_w"]:
+ self._image_dimensions_HW = image.shape[:2] # type: ignore[assignment]
self._calculate_paddings()
image = self._normalize_image_to_range_0_1(image)
@@ -213,7 +214,7 @@ def _pad_to_square(self, image: np.ndarray) -> np.ndarray:
right=self._padding_right,
borderType=cv2.BORDER_CONSTANT,
value=0,
- )
+ ) # type: ignore[call-overload]
def _resize_to_network_input_shape(self, image: np.ndarray) -> np.ndarray:
return cv2.resize(src=image, dsize=self._network_input_shape_WH)
diff --git a/bitbots_vision/bitbots_vision/vision_modules/yoeo/vision_components.py b/bitbots_vision/bitbots_vision/vision_modules/yoeo/vision_components.py
index 461e2e347..466fff7c9 100644
--- a/bitbots_vision/bitbots_vision/vision_modules/yoeo/vision_components.py
+++ b/bitbots_vision/bitbots_vision/vision_modules/yoeo/vision_components.py
@@ -7,6 +7,7 @@
import rclpy
from game_controller_hl_interfaces.msg import GameState
from rclpy.node import Node
+from rclpy.publisher import Publisher
from sensor_msgs.msg import Image
from soccer_vision_2d_msgs.msg import BallArray, GoalpostArray, Robot, RobotArray
@@ -31,7 +32,8 @@ def get(cls, config: dict) -> debug.DebugImage:
Get an instance of debug.DebugImage.
"""
if cls._new_debug_image_has_to_be_created(config):
- cls._create_new_debug_image(config)
+ return cls._create_new_debug_image(config)
+ assert cls._debug_image is not None, "Debug image not created!"
return cls._debug_image
@classmethod
@@ -41,9 +43,10 @@ def _new_debug_image_has_to_be_created(cls, config: dict) -> bool:
)
@classmethod
- def _create_new_debug_image(cls, config: dict) -> None:
+ def _create_new_debug_image(cls, config: dict) -> debug.DebugImage:
cls._debug_image = debug.DebugImage(config["component_debug_image_active"])
cls._config = config
+ return cls._debug_image
class IVisionComponent(ABC):
@@ -86,9 +89,11 @@ def configure(self, config: dict, debug_mode: bool) -> None:
self._yoeo_instance = object_manager.YOEOObjectManager.get()
def run(self, image_msg: Image) -> None:
+ assert self._yoeo_instance is not None, "YOEO handler instance not set!"
self._yoeo_instance.predict()
def set_image(self, image: np.ndarray) -> None:
+ assert self._yoeo_instance is not None, "YOEO handler instance not set!"
self._yoeo_instance.set_image(image)
@@ -103,7 +108,7 @@ def __init__(self, node: Node):
self._debug_image: Optional[debug.DebugImage] = None
self._debug_mode: bool = False
self._node: Node = node
- self._publisher: Optional[rclpy.publisher.Publisher] = None
+ self._publisher: Optional[Publisher] = None
def configure(self, config: dict, debug_mode: bool) -> None:
self._ball_detector = detectors.BallDetector(object_manager.YOEOObjectManager.get())
@@ -129,6 +134,7 @@ def run(self, image_msg: Image) -> None:
self._add_final_candidates_to_debug_image(final_candidates)
def _get_best_ball_candidates(self) -> list[candidate.Candidate]:
+ assert self._ball_detector is not None, "Ball detector not set!"
return self._ball_detector.get_top_candidates(count=self._config["ball_candidate_max_count"])
def _filter_by_candidate_threshold(self, candidates: list[candidate.Candidate]) -> list[candidate.Candidate]:
@@ -137,15 +143,19 @@ def _filter_by_candidate_threshold(self, candidates: list[candidate.Candidate])
def _publish_balls_message(self, image_msg: Image, candidates: list[candidate.Candidate]) -> None:
ball_messages = list(map(ros_utils.build_ball_msg, candidates))
balls_message = ros_utils.build_ball_array_msg(image_msg.header, ball_messages)
+ assert self._publisher is not None, "Publisher not set!"
self._publisher.publish(balls_message)
def _add_candidates_to_debug_image(self, candidates: list[candidate.Candidate]) -> None:
+ assert self._debug_image is not None, "Debug image cpomponent not set!"
self._debug_image.draw_ball_candidates(candidates, DebugImageComponent.Colors.ball, thickness=1)
def _add_candidates_within_convex_fb_to_debug_image(self, candidates: list[candidate.Candidate]) -> None:
+ assert self._debug_image is not None, "Debug image component not set!"
self._debug_image.draw_ball_candidates(candidates, DebugImageComponent.Colors.ball, thickness=2)
def _add_final_candidates_to_debug_image(self, candidates: list[candidate.Candidate]) -> None:
+ assert self._debug_image is not None, "Debug image component not set!"
self._debug_image.draw_ball_candidates(candidates, DebugImageComponent.Colors.ball, thickness=3)
def set_image(self, image: np.ndarray) -> None:
@@ -163,7 +173,7 @@ def __init__(self, node: Node):
self._debug_mode: bool = False
self._goalpost_detector: Optional[detectors.GoalpostDetector] = None
self._node: Node = node
- self._publisher: Optional[rclpy.publisher.Publisher] = None
+ self._publisher: Optional[Publisher] = None
def configure(self, config: dict, debug_mode: bool) -> None:
self._debug_image = DebugImageFactory.get(config)
@@ -183,21 +193,25 @@ def run(self, image_msg: Image) -> None:
self._publish_goalposts_message(image_msg, candidates)
if self._debug_mode:
- self._add_candidates_to_debug_image(self._goalpost_detector.get_candidates())
+ self._add_candidates_to_debug_image(self._get_candidates())
self._add_final_candidates_to_debug_image(candidates)
def _get_candidates(self) -> list[candidate.Candidate]:
+ assert self._goalpost_detector is not None, "Goalpost detector not set!"
return self._goalpost_detector.get_candidates()
def _publish_goalposts_message(self, image_msg: Image, candidates: list[candidate.Candidate]) -> None:
goalpost_messages = [ros_utils.build_goal_post_msg(candidate) for candidate in candidates]
goalposts_message = ros_utils.build_goal_post_array_msg(image_msg.header, goalpost_messages)
+ assert self._publisher is not None, "Publisher not set!"
self._publisher.publish(goalposts_message)
def _add_candidates_to_debug_image(self, candidates: list[candidate.Candidate]) -> None:
+ assert self._debug_image is not None, "Debug image component not set!"
self._debug_image.draw_robot_candidates(candidates, DebugImageComponent.Colors.goalposts, thickness=1)
def _add_final_candidates_to_debug_image(self, candidates: list[candidate.Candidate]) -> None:
+ assert self._debug_image is not None, "Debug image component not set!"
self._debug_image.draw_robot_candidates(candidates, DebugImageComponent.Colors.goalposts, thickness=3)
def set_image(self, image: np.ndarray) -> None:
@@ -215,7 +229,7 @@ def __init__(self, node: Node):
self._debug_mode: bool = False
self._line_detector: Optional[detectors.ISegmentation] = None
self._node: Node = node
- self._publisher: Optional[rclpy.publisher.Publisher] = None
+ self._publisher: Optional[Publisher] = None
def configure(self, config: dict, debug_mode: bool) -> None:
self._debug_image = DebugImageFactory.get(config)
@@ -231,6 +245,7 @@ def _register_publisher(self, new_config: dict) -> None:
)
def run(self, image_msg: Image) -> None:
+ assert self._line_detector is not None, "Line detector not set!"
line_mask = self._line_detector.get_mask_image()
self._publish_line_mask_msg(image_msg, line_mask)
@@ -239,9 +254,11 @@ def run(self, image_msg: Image) -> None:
def _publish_line_mask_msg(self, image_msg: Image, line_mask: np.ndarray) -> None:
line_mask_msg = ros_utils.build_image_msg(image_msg.header, line_mask, "8UC1")
+ assert self._publisher is not None, "Publisher not set!"
self._publisher.publish(line_mask_msg)
def _add_line_mask_to_debug_image(self, line_mask: np.ndarray) -> None:
+ assert self._debug_image is not None, "Debug image component not set!"
self._debug_image.draw_mask(line_mask, color=DebugImageComponent.Colors.lines)
def set_image(self, image: np.ndarray) -> None:
@@ -257,7 +274,7 @@ def __init__(self, node: Node):
self._config: dict = {}
self._field_detector: Optional[detectors.ISegmentation] = None
self._node: Node = node
- self._publisher: Optional[rclpy.publisher.Publisher] = None
+ self._publisher: Optional[Publisher] = None
def configure(self, config: dict, debug_mode: bool) -> None:
self._field_detector = detectors.FieldSegmentation(object_manager.YOEOObjectManager.get())
@@ -276,10 +293,12 @@ def run(self, image_msg: Image) -> None:
self._publish_field_mask_msg(image_msg, field_mask)
def _get_field_mask(self) -> np.ndarray:
+ assert self._field_detector is not None, "Field detector not set!"
return self._field_detector.get_mask_image()
def _publish_field_mask_msg(self, image_msg: Image, field_mask: np.ndarray) -> None:
field_mask_msg = ros_utils.build_image_msg(image_msg.header, field_mask, "8UC1")
+ assert self._publisher is not None, "Publisher not set!"
self._publisher.publish(field_mask_msg)
def set_image(self, image: np.ndarray) -> None:
@@ -301,7 +320,7 @@ def __init__(self, node: Node):
self._team_mates_detector: Optional[candidate.CandidateFinder] = None
self._node: Node = node
- self._publisher: Optional[rclpy.publisher.Publisher] = None
+ self._publisher: Optional[Publisher] = None
def configure(self, config: dict, debug_mode: bool) -> None:
own_color, opponent_color = self._determine_team_colors()
@@ -337,6 +356,7 @@ def run(self, image_msg: Image) -> None:
self._add_robots_to_debug_image()
def _add_team_mates_to(self, robots_msgs: list[Robot]) -> None:
+ assert self._team_mates_detector is not None, "Team mates detector not set!"
team_mate_candidates = self._team_mates_detector.get_candidates()
team_mate_candidate_messages = self._create_robots_messages(Robot().attributes.TEAM_OWN, team_mate_candidates)
robots_msgs.extend(team_mate_candidate_messages)
@@ -346,6 +366,7 @@ def _create_robots_messages(robot_type: Robot, candidates: list[candidate.Candid
return [ros_utils.build_robot_msg(obstacle_candidate, robot_type) for obstacle_candidate in candidates]
def _add_opponents_to(self, robot_msgs: list[Robot]) -> None:
+ assert self._opponents_detector is not None, "Opponents detector not set!"
opponent_candidates = self._opponents_detector.get_candidates()
opponent_candidate_messages = self._create_robots_messages(
Robot().attributes.TEAM_OPPONENT, opponent_candidates
@@ -353,6 +374,7 @@ def _add_opponents_to(self, robot_msgs: list[Robot]) -> None:
robot_msgs.extend(opponent_candidate_messages)
def _add_remaining_obstacles_to(self, robot_msgs: list[Robot]) -> None:
+ assert self._unknown_detector is not None, "Unknown detector not set!"
remaining_candidates = self._unknown_detector.get_candidates()
remaining_candidate_messages = self._create_robots_messages(
Robot().attributes.TEAM_UNKNOWN, remaining_candidates
@@ -361,6 +383,7 @@ def _add_remaining_obstacles_to(self, robot_msgs: list[Robot]) -> None:
def _publish_robots_message(self, image_msg: Image, robot_msgs: list[Robot]) -> None:
robots_msg = ros_utils.build_robot_array_msg(image_msg.header, robot_msgs)
+ assert self._publisher is not None, "Publisher not set!"
self._publisher.publish(robots_msg)
def _add_robots_to_debug_image(self) -> None:
@@ -369,35 +392,45 @@ def _add_robots_to_debug_image(self) -> None:
self._add_remaining_objects_to_debug_image()
def _add_team_mates_to_debug_image(self) -> None:
+ assert self._team_mates_detector is not None, "Team mates detector not set!"
team_mate_candidates = self._team_mates_detector.get_candidates()
+ assert self._debug_image is not None, "Debug image component not set!"
self._debug_image.draw_robot_candidates(
team_mate_candidates, DebugImageComponent.Colors.robot_team_mates, thickness=3
)
def _add_opponents_to_debug_image(self) -> None:
+ assert self._opponents_detector is not None, "Opponents detector not set!"
opponent_candidates = self._opponents_detector.get_candidates()
+ assert self._debug_image is not None, "Debug image component not set!"
self._debug_image.draw_robot_candidates(
opponent_candidates, DebugImageComponent.Colors.robot_opponents, thickness=3
)
def _add_remaining_objects_to_debug_image(self) -> None:
+ assert self._unknown_detector is not None, "Unknown detector not set!"
remaining_candidates = self._unknown_detector.get_candidates()
+ assert self._debug_image is not None, "Debug image component not set!"
self._debug_image.draw_robot_candidates(
remaining_candidates, DebugImageComponent.Colors.robot_unknown, thickness=3
)
def set_image(self, image: np.ndarray) -> None:
+ assert self._team_mates_detector is not None, "Team mates detector not set!"
self._team_mates_detector.set_image(image)
+ assert self._opponents_detector is not None, "Opponents detector not set!"
self._opponents_detector.set_image(image)
+ assert self._unknown_detector is not None, "Unknown detector not set!"
self._unknown_detector.set_image(image)
- def _configure_detectors(self, config: dict, own_color: int, opponent_color: int) -> None:
+ def _configure_detectors(self, config: dict, own_color: Optional[int], opponent_color: Optional[int]) -> None:
self._team_mates_detector = self._select_detector_based_on(own_color)
self._opponents_detector = self._select_detector_based_on(opponent_color)
self._unknown_detector = self._select_detector_based_on(None)
@classmethod
def _select_detector_based_on(cls, team_color: Optional[int]):
+ color_detector: detectors.DetectorTemplate
if team_color == GameState.BLUE:
color_detector = detectors.BlueRobotDetector(object_manager.YOEOObjectManager.get())
elif team_color == GameState.RED:
@@ -421,7 +454,7 @@ def __init__(self, node: Node):
self._robot_detector: Optional[candidate.CandidateFinder] = None
self._node: Node = node
- self._publisher: Optional[rclpy.publisher.Publisher] = None
+ self._publisher: Optional[Publisher] = None
def configure(self, config: dict, debug_mode: bool) -> None:
self._debug_image = DebugImageFactory.get(config)
@@ -447,23 +480,30 @@ def run(self, image_msg: Image) -> None:
self._add_robots_to_debug_image()
def _add_robots_to(self, robot_msgs: list[Robot]) -> None:
+ assert self._robot_detector is not None, "Robot detector not set!"
robot_candidates = self._robot_detector.get_candidates()
robot_candidate_messages = self._create_robot_messages(Robot().attributes.TEAM_UNKNOWN, robot_candidates)
robot_msgs.extend(robot_candidate_messages)
@staticmethod
- def _create_robot_messages(robot_type: Robot, candidates: list[candidate.Candidate]) -> list[Robot]:
+ def _create_robot_messages(
+ robot_type: ros_utils.T_RobotAttributes_Team, candidates: list[candidate.Candidate]
+ ) -> list[Robot]:
return [ros_utils.build_robot_msg(robot_candidate, robot_type) for robot_candidate in candidates]
def _publish_robots_message(self, image_msg: Image, robot_msgs: list[Robot]) -> None:
robots_msg = ros_utils.build_robot_array_msg(image_msg.header, robot_msgs)
+ assert self._publisher is not None, "Publisher not set!"
self._publisher.publish(robots_msg)
def _add_robots_to_debug_image(self) -> None:
+ assert self._robot_detector is not None, "Robot detector not set!"
robot_candidates = self._robot_detector.get_candidates()
+ assert self._debug_image is not None, "Debug image component not set!"
self._debug_image.draw_robot_candidates(robot_candidates, DebugImageComponent.Colors.robot_unknown, thickness=3)
def set_image(self, image: np.ndarray) -> None:
+ assert self._robot_detector is not None, "Robot detector not set!"
self._robot_detector.set_image(image)
@@ -488,7 +528,7 @@ def __init__(self, node):
self._config: dict = {}
self._node: Node = node
self._debug_image: Optional[debug.DebugImage] = None
- self._publisher: Optional[rclpy.publisher.Publisher] = None
+ self._publisher: Optional[Publisher] = None
def configure(self, config: dict, debug_mode: bool) -> None:
self._debug_image = DebugImageFactory.get(config)
@@ -507,10 +547,13 @@ def run(self, image_msg: Image) -> None:
self._publish_debug_image_msg(debug_image_msg)
def _create_debug_image_msg(self, image_msg: Image) -> Image:
+ assert self._debug_image is not None, "Debug image component not set!"
return ros_utils.build_image_msg(image_msg.header, self._debug_image.get_image(), "bgr8")
def _publish_debug_image_msg(self, debug_image_msg: Image) -> None:
+ assert self._publisher is not None, "Publisher not set!"
self._publisher.publish(debug_image_msg)
def set_image(self, image: np.ndarray) -> None:
+ assert self._debug_image is not None, "Debug image component not set!"
self._debug_image.set_image(image)
diff --git a/bitbots_vision/bitbots_vision/vision_modules/yoeo/yoeo_handlers.py b/bitbots_vision/bitbots_vision/vision_modules/yoeo/yoeo_handlers.py
index 3f3aef805..76cd313d0 100644
--- a/bitbots_vision/bitbots_vision/vision_modules/yoeo/yoeo_handlers.py
+++ b/bitbots_vision/bitbots_vision/vision_modules/yoeo/yoeo_handlers.py
@@ -97,7 +97,12 @@ class YOEOHandlerTemplate(IYOEOHandler):
"""
def __init__(
- self, config: dict, det_class_names: list[str], det_robot_class_ids: list[int], seg_class_names: list[str]
+ self,
+ config: dict,
+ model_directory: str,
+ det_class_names: list[str],
+ det_robot_class_ids: list[int],
+ seg_class_names: list[str],
):
logger.debug("Entering YOEOHandlerTemplate constructor")
@@ -149,7 +154,7 @@ def get_segmentation_mask_for(self, class_name: str):
def predict(self) -> None:
if self._prediction_has_to_be_updated():
logger.debug("Computing new prediction...")
-
+ assert self._image is not None, "No image set"
detections, segmentation = self._compute_new_prediction_for(self._image)
self._create_detection_candidate_lists_from(detections)
self._create_segmentation_masks_based_on(segmentation)
@@ -161,7 +166,7 @@ def _prediction_has_to_be_updated(self) -> bool:
def _create_detection_candidate_lists_from(self, detections: np.ndarray) -> None:
for detection in detections:
- c = Candidate.from_x1y1x2y2(*detection[0:4].astype(int), detection[4].astype(float))
+ c = Candidate.from_x1y1x2y2(*detection[0:4].astype(int), detection[4].astype(float)) # type: ignore[call-arg]
self._det_candidates[self._det_class_names[int(detection[5])]].append(c)
def _create_segmentation_masks_based_on(self, segmentation) -> None:
@@ -215,7 +220,7 @@ def __init__(
det_robot_class_ids: list[int],
seg_class_names: list[str],
):
- super().__init__(config, det_class_names, det_robot_class_ids, seg_class_names)
+ super().__init__(config, model_directory, det_class_names, det_robot_class_ids, seg_class_names)
logger.debug(f"Entering {self.__class__.__name__} constructor")
@@ -288,7 +293,7 @@ def __init__(
det_robot_class_ids: list[int],
seg_class_names: list[str],
):
- super().__init__(config, det_class_names, det_robot_class_ids, seg_class_names)
+ super().__init__(config, model_directory, det_class_names, det_robot_class_ids, seg_class_names)
logger.debug(f"Entering {self.__class__.__name__} constructor")
@@ -376,7 +381,7 @@ def __init__(
det_robot_class_ids: list[int],
seg_class_names: list[str],
):
- super().__init__(config, det_class_names, det_robot_class_ids, seg_class_names)
+ super().__init__(config, model_directory, det_class_names, det_robot_class_ids, seg_class_names)
logger.debug(f"Entering {self.__class__.__name__} constructor")
@@ -450,7 +455,7 @@ def __init__(
det_robot_class_ids: list[int],
seg_class_names: list[str],
):
- super().__init__(config, det_class_names, det_robot_class_ids, seg_class_names)
+ super().__init__(config, model_directory, det_class_names, det_robot_class_ids, seg_class_names)
logger.debug(f"Entering {self.__class__.__name__} constructor")
diff --git a/bitbots_vision/package.xml b/bitbots_vision/package.xml
index 740a5ded8..fa0dd650d 100644
--- a/bitbots_vision/package.xml
+++ b/bitbots_vision/package.xml
@@ -39,6 +39,7 @@
soccer_vision_2d_msgs
std_msgs
vision_opencv
+ ament_mypy
diff --git a/bitbots_vision/scripts/extract_from_rosbag.py b/bitbots_vision/scripts/extract_from_rosbag.py
index 2aa4ae3b1..e3de043b1 100755
--- a/bitbots_vision/scripts/extract_from_rosbag.py
+++ b/bitbots_vision/scripts/extract_from_rosbag.py
@@ -1,5 +1,7 @@
#!/usr/bin/env python3
+# type: ignore
+
import argparse
import os
@@ -9,8 +11,7 @@
from sensor_msgs.msg import Image
-def yes_or_no_input(question, default=None):
- # type: (str) -> bool
+def yes_or_no_input(question, default=None) -> bool | None:
"""
Prints a yes or no question and returns the answer.
diff --git a/bitbots_vision/setup.py b/bitbots_vision/setup.py
index 2e24e1962..421cb95ee 100755
--- a/bitbots_vision/setup.py
+++ b/bitbots_vision/setup.py
@@ -26,6 +26,7 @@
zip_safe=True,
keywords=["ROS"],
license="MIT",
+ tests_require=["pytest"],
entry_points={
"console_scripts": [
"vision = bitbots_vision.vision:main",
diff --git a/bitbots_vision/test/mypy.ini b/bitbots_vision/test/mypy.ini
new file mode 100644
index 000000000..6b19e8374
--- /dev/null
+++ b/bitbots_vision/test/mypy.ini
@@ -0,0 +1,5 @@
+# Global options:
+
+[mypy]
+check_untyped_defs = True
+ignore_missing_imports = True
diff --git a/bitbots_vision/test/test_mypy.py b/bitbots_vision/test/test_mypy.py
new file mode 100644
index 000000000..3dacd6040
--- /dev/null
+++ b/bitbots_vision/test/test_mypy.py
@@ -0,0 +1,10 @@
+from pathlib import Path
+
+import pytest
+from ament_mypy.main import main
+
+
+@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"
diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/__init__.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/__init__.py
+++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py
index 1dca18f39..f91316ab8 100755
--- a/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py
+++ b/bitbots_world_model/bitbots_ball_filter/bitbots_ball_filter/ball_filter.py
@@ -1,5 +1,5 @@
#! /usr/bin/env python3
-from typing import Optional, Tuple
+from typing import Optional
import numpy as np
import rclpy
@@ -81,7 +81,7 @@ def reset_ball(self) -> None:
self.ball_state_position = np.zeros(3)
self.ball_state_covariance = np.eye(3) * 1000
- def reset_filter_cb(self, req, response) -> Tuple[bool, str]:
+ def reset_filter_cb(self, _: Trigger.Request, response: Trigger.Response) -> Trigger.Response:
self.logger.info("Resetting bitbots ball filter...")
self.reset_ball()
response.success = True
@@ -122,12 +122,12 @@ def ball_callback(self, msg: BallArray) -> None:
diff = numpify(ball_transform.point) - self.ball_state_position
if abs(diff[0]) < ignore_threshold_x and abs(diff[1]) < ignore_threshold_y:
# Store the ball relative to the robot, the ball in the filter frame and the distance to the filter estimate
- filtered_balls.append((ball, ball_transform, np.linalg.norm(diff)))
+ filtered_balls.append((ball, ball_transform, float(np.linalg.norm(diff))))
# Select the ball with closest distance to the filter estimate
ball_msg, ball_measurement_map, _ = min(filtered_balls, key=lambda x: x[2], default=(None, None, 0))
# Only update the measurement if we have a ball that is close enough to the filter's estimate
- if ball_measurement_map is not None:
+ if ball_measurement_map is not None and ball_msg is not None:
# Estimate the covariance of the measurement
# Calculate the distance from the robot to the ball
distance = np.linalg.norm(numpify(ball_msg.center))
@@ -179,6 +179,7 @@ def _get_transform(
return self.tf_buffer.transform(point_stamped, frame, timeout=Duration(nanoseconds=int(timeout * (10**9))))
except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e:
self.logger.warning(str(e))
+ return None
def update_params(self) -> None:
"""
@@ -224,8 +225,8 @@ def is_estimate_in_fov(self, header: Header) -> bool:
# Make sure that the transformed pixel is on the sensor and not too close to the border
border_fraction = self.config.filter.covariance.negative_observation.ignore_border
border_px = np.array([self.camera_info.width, self.camera_info.height]) / 2 * border_fraction
- in_fov_horizontal = border_px[0] < pixel[0] <= self.camera_info.width - border_px[0]
- in_fov_vertical = border_px[1] < pixel[1] <= self.camera_info.height - border_px[1]
+ in_fov_horizontal = bool(border_px[0] < pixel[0] <= self.camera_info.width - border_px[0])
+ in_fov_vertical = bool(border_px[1] < pixel[1] <= self.camera_info.height - border_px[1])
return in_fov_horizontal and in_fov_vertical
def filter_step(self) -> None:
diff --git a/bitbots_world_model/bitbots_ball_filter/package.xml b/bitbots_world_model/bitbots_ball_filter/package.xml
index 7c3cf3f1b..85f944fa4 100644
--- a/bitbots_world_model/bitbots_ball_filter/package.xml
+++ b/bitbots_world_model/bitbots_ball_filter/package.xml
@@ -22,6 +22,7 @@
std_msgs
std_srvs
tf2_geometry_msgs
+ ament_mypy
diff --git a/bitbots_world_model/bitbots_ball_filter/setup.py b/bitbots_world_model/bitbots_ball_filter/setup.py
index 56e0df152..c9ed710f2 100644
--- a/bitbots_world_model/bitbots_ball_filter/setup.py
+++ b/bitbots_world_model/bitbots_ball_filter/setup.py
@@ -27,6 +27,7 @@
zip_safe=True,
keywords=["ROS"],
license="MIT",
+ tests_require=["pytest"],
entry_points={
"console_scripts": [
"ball_filter = bitbots_ball_filter.ball_filter:main",
diff --git a/bitbots_world_model/bitbots_ball_filter/test/mypy.ini b/bitbots_world_model/bitbots_ball_filter/test/mypy.ini
new file mode 100644
index 000000000..19bad04b6
--- /dev/null
+++ b/bitbots_world_model/bitbots_ball_filter/test/mypy.ini
@@ -0,0 +1,4 @@
+# Global options:
+
+[mypy]
+ignore_missing_imports = true
diff --git a/bitbots_world_model/bitbots_ball_filter/test/test_mypy.py b/bitbots_world_model/bitbots_ball_filter/test/test_mypy.py
new file mode 100644
index 000000000..3dacd6040
--- /dev/null
+++ b/bitbots_world_model/bitbots_ball_filter/test/test_mypy.py
@@ -0,0 +1,10 @@
+from pathlib import Path
+
+import pytest
+from ament_mypy.main import main
+
+
+@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"
diff --git a/bitbots_world_model/bitbots_robot_filter/bitbots_robot_filter/__init__.py b/bitbots_world_model/bitbots_robot_filter/bitbots_robot_filter/__init__.py
index e69de29bb..4d2e15007 100644
--- a/bitbots_world_model/bitbots_robot_filter/bitbots_robot_filter/__init__.py
+++ b/bitbots_world_model/bitbots_robot_filter/bitbots_robot_filter/__init__.py
@@ -0,0 +1,4 @@
+# Setting up runtime type checking for this package
+from beartype.claw import beartype_this_package
+
+beartype_this_package()
diff --git a/bitbots_world_model/bitbots_robot_filter/package.xml b/bitbots_world_model/bitbots_robot_filter/package.xml
index 72e3647c8..a0b6003aa 100644
--- a/bitbots_world_model/bitbots_robot_filter/package.xml
+++ b/bitbots_world_model/bitbots_robot_filter/package.xml
@@ -19,6 +19,7 @@
tf2_geometry_msgs
tf2_ros
+ ament_mypy
ament_copyright
ament_flake8
ament_pep257
diff --git a/bitbots_world_model/bitbots_robot_filter/test/mypy.ini b/bitbots_world_model/bitbots_robot_filter/test/mypy.ini
new file mode 100644
index 000000000..19bad04b6
--- /dev/null
+++ b/bitbots_world_model/bitbots_robot_filter/test/mypy.ini
@@ -0,0 +1,4 @@
+# Global options:
+
+[mypy]
+ignore_missing_imports = true
diff --git a/bitbots_world_model/bitbots_robot_filter/test/test_mypy.py b/bitbots_world_model/bitbots_robot_filter/test/test_mypy.py
new file mode 100644
index 000000000..3dacd6040
--- /dev/null
+++ b/bitbots_world_model/bitbots_robot_filter/test/test_mypy.py
@@ -0,0 +1,10 @@
+from pathlib import Path
+
+import pytest
+from ament_mypy.main import main
+
+
+@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"
diff --git a/pyproject.toml b/pyproject.toml
index 6909aae00..b848a36d3 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -3,7 +3,7 @@ line-length = 120
[tool.ruff.lint]
# Never enforce `E501` (line length violations)
-ignore = ["E501", "UP007"]
+ignore = ["E501", "UP007", "F722"]
# Additionally enable the following rules
# - pycodestyle warnings (`W`)
# - flake8-bugbear warnings (`B`)
diff --git a/requirements/common.txt b/requirements/common.txt
index 79e894b3e..046b3efa4 100644
--- a/requirements/common.txt
+++ b/requirements/common.txt
@@ -4,6 +4,9 @@ transforms3d==0.4.1
git+https://github.com/Flova/pyastar2d
git+https://github.com/bit-bots/YOEO
simpleeval
+beartype
+jaxtyping
+mypy
# The following dependencies are only needed for rl walking and we don't actively use them currently
#git+https://github.com/bit-bots/deep_quintic.git