Skip to content

Commit

Permalink
PEP 585 compliance (#658)
Browse files Browse the repository at this point in the history
  • Loading branch information
Flova authored Jan 20, 2025
2 parents a851e49 + 5a7bd3a commit 29f0423
Show file tree
Hide file tree
Showing 15 changed files with 75 additions and 101 deletions.
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
import math
from typing import List, Optional, Tuple
from typing import Optional

import numpy as np
import tf2_ros as tf2
Expand Down Expand Up @@ -132,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[int, int]:
def field_2_costmap_coord(self, x: float, y: float) -> tuple[int, int]:
"""
Converts a field position to the corresponding indices for the costmap.
Expand Down Expand Up @@ -209,7 +209,7 @@ def calc_base_costmap(self) -> None:
0 : self.field_width + self.map_margin * 2 : (self.field_width + self.map_margin * 2) * 10j, # type: ignore[misc]
]

fix_points: List[Tuple[Tuple[float, float], float]] = []
fix_points: list[tuple[tuple[float, float], float]] = []

# Add base points
fix_points.extend(
Expand Down Expand Up @@ -284,7 +284,7 @@ def calc_base_costmap(self) -> None:
self.base_costmap = base_costmap
self.costmap = self.base_costmap.copy()

def get_gradient_at_field_position(self, x: float, y: float) -> Tuple[float, float]:
def get_gradient_at_field_position(self, x: float, y: float) -> tuple[float, float]:
"""
Gets the gradient tuple at a given field position
:param x: Field coordinate in the x direction
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
from typing import List, Literal, Optional, Tuple
from typing import Literal, Optional

import numpy as np
from bitbots_utils.utils import get_parameters_from_other_node
Expand Down Expand Up @@ -142,7 +142,7 @@ def set_action(self, action: int) -> None:
self.strategy.action = action
self.action_update = self._node.get_clock().now().nanoseconds / 1e9

def get_action(self) -> Tuple[int, float]:
def get_action(self) -> tuple[int, float]:
return self.strategy.action, self.action_update

def set_role(self, role: Literal["goalie", "offense", "defense"]) -> None:
Expand All @@ -154,7 +154,7 @@ def set_role(self, role: Literal["goalie", "offense", "defense"]) -> None:
self.strategy.role = self.roles_mapping[role]
self.role_update = self._node.get_clock().now().nanoseconds / 1e9

def get_role(self) -> Tuple[int, float]:
def get_role(self) -> tuple[int, float]:
return self.strategy.role, self.role_update

def set_kickoff_strategy(
Expand All @@ -164,10 +164,10 @@ def set_kickoff_strategy(
self.strategy.offensive_side = strategy
self.strategy_update = self._node.get_clock().now().nanoseconds / 1e9

def get_kickoff_strategy(self) -> Tuple[int, float]:
def get_kickoff_strategy(self) -> tuple[int, float]:
return self.strategy.offensive_side, self.strategy_update

def get_active_teammate_poses(self, count_goalies: bool = False) -> List[Pose]:
def get_active_teammate_poses(self, count_goalies: bool = False) -> list[Pose]:
"""Returns the poses of all playing robots"""
poses = []
data: TeamData
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,4 @@
import math
from typing import Tuple

import numpy as np
import tf2_ros as tf2
Expand Down Expand Up @@ -90,7 +89,7 @@ def ball_has_been_seen(self) -> bool:
"""Returns true if we or a teammate are reasonably sure that we have seen the ball"""
return self.ball_seen_self() or self._blackboard.team_data.teammate_ball_is_valid()

def get_ball_position_xy(self) -> Tuple[float, float]:
def get_ball_position_xy(self) -> tuple[float, float]:
"""Return the ball saved in the map frame, meaning the absolute position of the ball on the field"""
ball = self.get_best_ball_point_stamped()
return ball.point.x, ball.point.y
Expand All @@ -117,7 +116,7 @@ def get_best_ball_point_stamped(self) -> PointStamped:
self.debug_publisher_which_ball.publish(Header(stamp=own_ball.header.stamp, frame_id="own_ball_map"))
return own_ball

def get_ball_position_uv(self) -> Tuple[float, float]:
def get_ball_position_uv(self) -> tuple[float, float]:
"""
Returns the ball position relative to the robot in the base_footprint frame.
U and V are returned, where positive U is forward, positive V is to the left.
Expand Down Expand Up @@ -183,18 +182,18 @@ def forget_ball(self) -> None:
# Goal #
########

def get_map_based_opp_goal_center_uv(self) -> Tuple[float, float]:
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) -> Tuple[float, float]:
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) -> Tuple[float, float]:
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) -> Tuple[float, float]:
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) -> float:
Expand All @@ -210,19 +209,19 @@ 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) -> Tuple[float, float]:
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) -> Tuple[float, float]:
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)

########
# Pose #
########

def get_current_position(self) -> 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.
Expand Down Expand Up @@ -261,7 +260,7 @@ def get_current_position_transform(self) -> TransformStamped:
# Common #
##########

def get_uv_from_xy(self, x: float, y: float) -> 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]
Expand All @@ -271,7 +270,7 @@ def get_uv_from_xy(self, x: float, y: float) -> Tuple[float, float]:
v = math.cos(theta) * y2 - math.sin(theta) * x2
return u, v

def get_xy_from_uv(self, u: float, v: float) -> Tuple[float, float]:
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
Expand Down
5 changes: 2 additions & 3 deletions bitbots_misc/bitbots_tts/bitbots_tts/tts.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import subprocess
import time
import traceback
from typing import List, Tuple

import rclpy
import requests
Expand Down Expand Up @@ -44,7 +43,7 @@ def __init__(self) -> None:
super().__init__("tts_speaker")

# Class Variables
self.prio_queue: List[Tuple[str, int]] = []
self.prio_queue: list[tuple[str, int]] = []
self.speak_enabled = None
self.print_say = None
self.message_level = None
Expand Down Expand Up @@ -77,7 +76,7 @@ def __init__(self) -> None:
# Start processing the queue
self.create_timer(0.1, self.run_speaker, callback_group=MutuallyExclusiveCallbackGroup())

def on_set_parameters(self, parameters: List[Parameter]) -> SetParametersResult:
def on_set_parameters(self, parameters: list[Parameter]) -> SetParametersResult:
"""Callback for parameter changes."""
for parameter in parameters:
if parameter.name == "print":
Expand Down
29 changes: 13 additions & 16 deletions bitbots_misc/bitbots_utils/bitbots_utils/utils.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
import os
from typing import Any, Dict, List
from typing import Any

import rclpy
import yaml
Expand All @@ -24,7 +24,7 @@ def read_urdf(robot_name: str) -> str:
return urdf


def load_moveit_parameter(robot_name: str) -> List[ParameterMsg]:
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.",
Expand All @@ -44,7 +44,7 @@ def load_moveit_parameter(robot_name: str) -> List[ParameterMsg]:
return moveit_parameters


def get_parameters_from_ros_yaml(node_name: str, parameter_file: str, use_wildcard: bool) -> List[ParameterMsg]:
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)
Expand All @@ -70,24 +70,21 @@ def get_parameters_from_ros_yaml(node_name: str, parameter_file: str, use_wildca
return parse_parameter_dict(namespace="", parameter_dict=param_dict)


def get_parameters_from_plain_yaml(parameter_file, namespace="") -> List[ParameterMsg]:
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)


def get_parameter_dict(node: Node, prefix: str) -> Dict:
def get_parameter_dict(node: Node, prefix: str) -> dict:
"""
Get a dictionary of parameters from a node.
:param node: Node to get parameters from
:type node: Node
:param prefix: Prefix for the nesting of the parameter query (e.g. 'body.nice_param.index' could have the prefix 'body')
:type prefix: str
:return: Dictionary of parameters without prefix nesting
:rtype: Dict
"""
parameter_config: Dict[str, Parameter] = node.get_parameters_by_prefix(prefix)
parameter_config: dict[str, Parameter] = node.get_parameters_by_prefix(prefix)
config = dict()
for param in parameter_config.values():
# Split separated keys into nested dicts
Expand All @@ -112,8 +109,8 @@ def get_parameter_dict(node: Node, prefix: str) -> Dict:


def get_parameters_from_other_node(
own_node: Node, other_node_name: str, parameter_names: List[str], service_timeout_sec: float = 20.0
) -> Dict:
own_node: Node, other_node_name: str, parameter_names: list[str], service_timeout_sec: float = 20.0
) -> dict:
"""
Used to receive parameters from other running nodes.
Returns a dict with requested parameter name as dict key and parameter value as dict value.
Expand All @@ -135,8 +132,8 @@ def get_parameters_from_other_node(


def get_parameters_from_other_node_sync(
own_node: Node, other_node_name: str, parameter_names: List[str], service_timeout_sec: float = 20.0
) -> Dict:
own_node: Node, other_node_name: str, parameter_names: list[str], service_timeout_sec: float = 20.0
) -> dict:
"""
Used to receive parameters from other running nodes. It does not use async internally.
It should not be used in callback functions, but it is a bit more reliable than the async version.
Expand All @@ -159,10 +156,10 @@ def get_parameters_from_other_node_sync(
def set_parameters_of_other_node(
own_node: Node,
other_node_name: str,
parameter_names: List[str],
parameter_values: List[Any],
parameter_names: list[str],
parameter_values: list[Any],
service_timeout_sec: float = 20.0,
) -> List[bool]:
) -> list[bool]:
"""
Used to set parameters of another running node.
Returns a list of booleans indicating success or failure.
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
from typing import List, Optional
from typing import Optional

import numpy
from bitbots_utils.utils import get_parameters_from_other_node_sync
Expand Down Expand Up @@ -117,8 +117,8 @@ def __init__(self, node: Node):

# Pressure sensors
# Initialize values high to prevent wrongly thinking the robot is picked up during start or in simulation
self.pressures: List[float] = [100.0] * 8
self.previous_pressures: List[float] = self.pressures.copy()
self.pressures: list[float] = [100.0] * 8
self.previous_pressures: list[float] = self.pressures.copy()
self.last_different_pressure_state_time: Optional[Time] = None

# Diagnostics state
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
import socket
import struct
import threading
from typing import List, Optional, Tuple
from typing import Optional

import rclpy
from ament_index_python.packages import get_package_share_directory
Expand Down Expand Up @@ -84,8 +84,8 @@ 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.0, 0.0)
self.ball_covariance: List[double] = []
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)
self.time_to_ball: Optional[float] = None
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
import socket
from ipaddress import IPv4Address
from typing import List, Optional
from typing import Optional

from rclpy.node import Node

Expand All @@ -15,7 +15,7 @@ def __init__(self, node: Node, logger, team_id, robot_id):

if self.target_ip.is_loopback:
# local mode on loopback device, bind to port depending on bot id and team id
local_target_ports: List[int] = node.get_parameter("local_target_ports").value
local_target_ports: list[int] = node.get_parameter("local_target_ports").value
self.target_ports = [port + 10 * team_id for port in local_target_ports]
self.receive_port = self.target_ports[robot_id - 1]
else:
Expand Down
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
from typing import Iterable, Sequence, Tuple
from typing import Iterable, Sequence

import numpy as np
import transforms3d
Expand Down Expand Up @@ -85,7 +85,7 @@ def convert_robot_pose(self, message_robot_pose: Proto.Robot) -> PoseWithCovaria

return robot

def convert_to_quat(self, euler_angles: Tuple[float, float, float]):
def convert_to_quat(self, euler_angles: tuple[float, float, float]):
return transforms3d.euler.euler2quat(*euler_angles)

def convert_to_row_major_covariance(
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
import math
from typing import Callable, Optional, Tuple
from typing import Callable, Optional

import numpy as np
import transforms3d
Expand Down Expand Up @@ -67,7 +67,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_velocity: tuple[float, float, float],
ball_covariance: Float64[np.ndarray, "36"],
message,
):
Expand Down
Loading

0 comments on commit 29f0423

Please sign in to comment.