diff --git a/.vscode/settings.json b/.vscode/settings.json index ad9463f11..aac9f7c1b 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -215,7 +215,8 @@ "regex": "cpp", "future": "cpp", "*.ipp": "cpp", - "span": "cpp" + "span": "cpp", + "ranges": "cpp" }, // Tell the ROS extension where to find the setup.bash // This also utilizes the COLCON_WS environment variable, which needs to be set 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/ball_kick_area.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/ball_kick_area.py index 3aa9147b8..54d02574a 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/ball_kick_area.py +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/ball_kick_area.py @@ -25,6 +25,9 @@ def perform(self, reevaluate=False): ball_position = self.blackboard.world_model.get_ball_position_uv() self.publish_debug_data("ball_position", {"u": ball_position[0], "v": ball_position[1]}) + self.publish_debug_data( + "smoothing_close", f"{self.no_near_decisions} ({self.no_near_decisions / self.smoothing * 10}%)" + ) # Check if the ball is in the enter area if 0 <= ball_position[0] <= self.kick_x_enter and 0 <= abs(ball_position[1]) <= self.kick_y_enter: 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/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index 4b2e179a5..cd65c1175 100644 --- a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml +++ b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml @@ -146,7 +146,7 @@ body_behavior: dribble_ball_distance_threshold: 0.5 dribble_kick_angle: 0.6 - kick_decision_smoothing: 5.0 + kick_decision_smoothing: 1.0 ################## # costmap params # diff --git a/bitbots_behavior/bitbots_body_behavior/package.xml b/bitbots_behavior/bitbots_body_behavior/package.xml index 6a12bc9ce..c0256d7dc 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 python3-pytest 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/CMakeLists.txt b/bitbots_misc/bitbots_tf_buffer/CMakeLists.txt deleted file mode 100644 index b4922e8d6..000000000 --- a/bitbots_misc/bitbots_tf_buffer/CMakeLists.txt +++ /dev/null @@ -1,53 +0,0 @@ -cmake_minimum_required(VERSION 3.9) -project(bitbots_tf_buffer) - -# Add support for C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -# Include CheckIPOSupported module -include(CheckIPOSupported) - -# Check if IPO is supported -check_ipo_supported(RESULT ipo_supported) -if(ipo_supported) - set(CMAKE_INTERPROCEDURAL_OPTIMIZATION TRUE) - # Set the number of parallel LTO jobs - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -flto=4") -endif() - -set(PYBIND11_PYTHON_VERSION 3) -set(PYBIND11_FINDPYTHON ON) -find_package(ament_cmake REQUIRED) -find_package(backward_ros REQUIRED) -find_package(pybind11 REQUIRED) -find_package(Python3 REQUIRED COMPONENTS Interpreter Development) -find_package(rcl REQUIRED) -find_package(rclcpp REQUIRED) -find_package(ros2_python_extension REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) - -add_compile_options(-Wall -Wno-unused) - -pybind11_add_module(cpp_wrapper SHARED src/bitbots_tf_buffer.cpp) - -ament_target_dependencies( - cpp_wrapper - PUBLIC - rclcpp - rcl - tf2 - tf2_ros - ros2_python_extension) - -target_link_libraries(cpp_wrapper PRIVATE pybind11::module) - -ament_python_install_package(${PROJECT_NAME}) - -ament_get_python_install_dir(PYTHON_INSTALL_DIR) - -install(TARGETS cpp_wrapper DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}") - -ament_package() diff --git a/bitbots_misc/bitbots_tf_buffer/README.md b/bitbots_misc/bitbots_tf_buffer/README.md deleted file mode 100644 index c9d7e4bf1..000000000 --- a/bitbots_misc/bitbots_tf_buffer/README.md +++ /dev/null @@ -1,19 +0,0 @@ -# bitbots_tf_buffer - -This is a nearly drop-in replacement for `tf2_ros.Buffer` in Python. It wraps a C++ node that holds the tf buffer and listener. The interface should be the same as the original `tf2_ros.Buffer`, except that we need to pass a reference to the node to the constructor. - -## Usage - -- Replace `from tf2_ros import Buffer, TransformListener` with `from bitbots_tf_buffer import -Buffer`. -- Remove the `TransformListener` from the code -- Pass a reference to the node to the constructor of `Buffer`: - -```python -from bitbots_tf_buffer import Buffer - -class MyNode(Node): - def __init__(self): - super().__init__('my_node') - self.tf_buffer = Buffer(self) -``` diff --git a/bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py b/bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py deleted file mode 100644 index 33230d76e..000000000 --- a/bitbots_misc/bitbots_tf_buffer/bitbots_tf_buffer/__init__.py +++ /dev/null @@ -1,54 +0,0 @@ -from typing import Optional - -import tf2_ros as tf2 -from builtin_interfaces.msg import Duration as DurationMsg -from builtin_interfaces.msg import Time as TimeMsg -from geometry_msgs.msg import TransformStamped -from rclpy.duration import Duration -from rclpy.serialization import deserialize_message, serialize_message -from rclpy.time import Time - -from bitbots_tf_buffer.cpp_wrapper import Buffer as CppBuffer - - -class Buffer(tf2.BufferCore, tf2.BufferInterface): - """ - Buffer class that wraps the C++ implementation of the tf2 buffer and listener. - It spawns a new node with the suffix "_tf" to handle the C++ side of the ROS communication. - """ - - def __init__(self, node, cache_time: Optional[Duration] = None, *args, **kwargs): - if cache_time is None: - cache_time = Duration(seconds=10.0) - - tf2.BufferCore.__init__(self, cache_time) - tf2.BufferInterface.__init__(self) - - self._impl = CppBuffer(serialize_message(Duration.to_msg(cache_time)), node) - - def lookup_transform( - self, target_frame: str, source_frame: str, time: Time, timeout: Optional[Duration] = None - ) -> TransformStamped: - # Handle timeout as None - timeout = timeout or Duration() - # Call cpp implementation - transform_str = self._impl.lookup_transform( - target_frame, - source_frame, - serialize_message(time if isinstance(time, TimeMsg) else Time.to_msg(time)), - serialize_message(timeout if isinstance(timeout, DurationMsg) else Duration.to_msg(timeout)), - ) - return deserialize_message(transform_str, TransformStamped) - - def can_transform( - self, target_frame: str, source_frame: str, time: Time, timeout: Optional[Duration] = None - ) -> bool: - # Handle timeout as None - timeout = timeout or Duration() - # Call cpp implementation - return self._impl.can_transform( - target_frame, - source_frame, - serialize_message(time if isinstance(time, TimeMsg) else Time.to_msg(time)), - serialize_message(timeout if isinstance(timeout, DurationMsg) else Duration.to_msg(timeout)), - ) diff --git a/bitbots_misc/bitbots_tf_buffer/package.xml b/bitbots_misc/bitbots_tf_buffer/package.xml deleted file mode 100644 index 941051664..000000000 --- a/bitbots_misc/bitbots_tf_buffer/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - bitbots_tf_buffer - 1.0.0 - Drop-in replacement for tf listener to improve performance - - Timon Engelke - - MIT - - ament_cmake - pybind11_vendor - ros2_python_extension - tf2_ros - tf2 - - - ament_cmake - - diff --git a/bitbots_misc/bitbots_tf_buffer/setup.py b/bitbots_misc/bitbots_tf_buffer/setup.py deleted file mode 100644 index 0c4c1d8ee..000000000 --- a/bitbots_misc/bitbots_tf_buffer/setup.py +++ /dev/null @@ -1,9 +0,0 @@ -from setuptools import setup - -setup( - name="bitbots_tf_buffer", - packages=["bitbots_tf_buffer"], - zip_safe=True, - keywords=["ROS"], - license="MIT", -) diff --git a/bitbots_misc/bitbots_tf_buffer/src/bitbots_tf_buffer.cpp b/bitbots_misc/bitbots_tf_buffer/src/bitbots_tf_buffer.cpp deleted file mode 100644 index ade8337c0..000000000 --- a/bitbots_misc/bitbots_tf_buffer/src/bitbots_tf_buffer.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace py = pybind11; - -class Buffer { - public: - Buffer(py::bytes duration_raw, py::object node) { - // initialize rclcpp if not already done - if (!rclcpp::contexts::get_global_default_context()->is_valid()) { - rclcpp::init(0, nullptr); - } - - // Register the tf2 exceptions, so they can be caught in Python as expected - auto py_tf2_ros = py::module::import("tf2_ros"); - py::register_local_exception(py_tf2_ros, "LookupExceptionCpp", - py_tf2_ros.attr("LookupException")); - py::register_local_exception(py_tf2_ros, "ConnectivityExceptionCpp", - py_tf2_ros.attr("ConnectivityException")); - py::register_local_exception(py_tf2_ros, "ExtrapolationExceptionCpp", - py_tf2_ros.attr("ExtrapolationException")); - py::register_local_exception(py_tf2_ros, "InvalidArgumentExceptionCpp", - py_tf2_ros.attr("InvalidArgumentException")); - py::register_local_exception(py_tf2_ros, "TimeoutExceptionCpp", - py_tf2_ros.attr("TimeoutException")); - - // get node name from python node object - rcl_node_t *node_handle = - static_cast(reinterpret_cast(node.attr("handle").attr("pointer").cast())); - const char *node_name = rcl_node_get_name(node_handle); - // create node with name _tf - node_ = std::make_shared((std::string(node_name) + "_tf").c_str()); - - // Get buffer duration from python duration - auto duration = - tf2_ros::fromMsg(ros2_python_extension::fromPython(duration_raw)); - - // create subscribers - buffer_ = std::make_shared(this->node_->get_clock(), duration); - buffer_->setUsingDedicatedThread(true); - listener_ = std::make_shared(*buffer_, node_, false); - - // create executor and start thread spinning the executor - executor_ = std::make_shared(); - executor_->add_node(node_); - thread_ = std::make_shared([this]() { - while (rclcpp::ok()) { - executor_->spin_once(); - } - }); - } - - py::bytes lookup_transform(py::str target_frame, py::str source_frame, py::bytes time_raw, py::bytes timeout_raw) { - // Convert python objects to C++ objects - const std::string target_frame_str = target_frame.cast(); - const std::string source_frame_str = source_frame.cast(); - const rclcpp::Time time_msg{ros2_python_extension::fromPython(time_raw)}; - const rclcpp::Duration timeout{ros2_python_extension::fromPython(timeout_raw)}; - - // Lookup transform - auto transform = buffer_->lookupTransform(target_frame_str, source_frame_str, time_msg, timeout); - - // Convert C++ object back to python object - return ros2_python_extension::toPython(transform); - } - - bool can_transform(py::str target_frame, py::str source_frame, py::bytes time_raw, py::bytes timeout_raw) { - // Convert python objects to C++ objects - const std::string target_frame_str = target_frame.cast(); - const std::string source_frame_str = source_frame.cast(); - const rclcpp::Time time_msg{ros2_python_extension::fromPython(time_raw)}; - const rclcpp::Duration timeout{ros2_python_extension::fromPython(timeout_raw)}; - // Check if transform can be looked up - return buffer_->canTransform(target_frame_str, source_frame_str, time_msg, timeout); - } - - // destructor - ~Buffer() { - // the executor finishes when rclcpp is shutdown, so the thread can be joined - rclcpp::shutdown(); - thread_->join(); - } - - private: - rclcpp::Serialization time_serializer_; - rclcpp::Serialization duration_serializer_; - rclcpp::Serialization transform_serializer_; - - std::shared_ptr node_; - std::shared_ptr buffer_; - std::shared_ptr listener_; - std::shared_ptr thread_; - std::shared_ptr executor_; -}; - -PYBIND11_MODULE(cpp_wrapper, m) { - py::class_>(m, "Buffer") - .def(py::init()) - .def("lookup_transform", &Buffer::lookup_transform) - .def("can_transform", &Buffer::can_transform); -} 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 40019b786..2801efdac 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 python3-pytest diff --git a/bitbots_motion/bitbots_animation_rqt/setup.py b/bitbots_motion/bitbots_animation_rqt/setup.py index 639dc194a..d672f0aed 100644 --- a/bitbots_motion/bitbots_animation_rqt/setup.py +++ b/bitbots_motion/bitbots_animation_rqt/setup.py @@ -14,6 +14,7 @@ install_requires=["setuptools"], tests_require=["pytest"], 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 77427c8ef..3b00445bf 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 python3-pytest diff --git a/bitbots_motion/bitbots_animation_server/setup.py b/bitbots_motion/bitbots_animation_server/setup.py index c41689c5f..3b52c7026 100644 --- a/bitbots_motion/bitbots_animation_server/setup.py +++ b/bitbots_motion/bitbots_animation_server/setup.py @@ -22,6 +22,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_dynup/include/bitbots_dynup/dynup_node.hpp b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp index 6909337a9..ae214c2ec 100644 --- a/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp +++ b/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_node.hpp @@ -115,6 +115,7 @@ class DynupNode { double start_time_ = 0; bool server_free_ = true; bool debug_ = false; + bool cancel_goal_ = false; // TF2 related things tf2_ros::Buffer tf_buffer_; diff --git a/bitbots_motion/bitbots_dynup/src/dynup_node.cpp b/bitbots_motion/bitbots_dynup/src/dynup_node.cpp index 3316a348e..a37f009f3 100644 --- a/bitbots_motion/bitbots_dynup/src/dynup_node.cpp +++ b/bitbots_motion/bitbots_dynup/src/dynup_node.cpp @@ -215,6 +215,10 @@ rclcpp_action::GoalResponse DynupNode::goalCb(const rclcpp_action::GoalUUID &uui server_free_ = false; return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } else { + if (goal->from_hcm) { + cancel_goal_ = true; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } RCLCPP_WARN(node_->get_logger(), "Dynup is busy, goal rejected!"); return rclcpp_action::GoalResponse::REJECT; } @@ -259,11 +263,20 @@ void DynupNode::loopEngine(int loop_rate, std::shared_ptr goal_ /* Do the loop as long as nothing cancels it */ while (rclcpp::ok()) { rclcpp::Time startTime = node_->get_clock()->now(); + // This is used when the cancelation is requested externally if (goal_handle->is_canceling()) { goal_handle->canceled(result); RCLCPP_INFO(node_->get_logger(), "Goal canceled"); return; } + // This is used when the dynup server descides to cancel itself + if (cancel_goal_) { + result->successful = false; + goal_handle->abort(result); + server_free_ = true; + cancel_goal_ = false; + return; + } msg = step(getTimeDelta()); auto feedback = std::make_shared(); feedback->percent_done = engine_.getPercentDone(); @@ -282,9 +295,12 @@ void DynupNode::loopEngine(int loop_rate, std::shared_ptr goal_ if (msg.joint_names.empty()) { continue; } + msg.from_hcm = goal_handle->get_goal()->from_hcm; joint_goal_publisher_->publish(msg); node_->get_clock()->sleep_until(startTime + rclcpp::Duration::from_nanoseconds(1e9 / loop_rate)); } + // Wether we canceled the goal or we finished cleanly the cancelation request is not valid anymore + cancel_goal_ = false; } bitbots_dynup::msg::DynupPoses DynupNode::getCurrentPoses() { diff --git a/bitbots_motion/bitbots_hcm/CMakeLists.txt b/bitbots_motion/bitbots_hcm/CMakeLists.txt index 2c24196eb..b0581fb82 100644 --- a/bitbots_motion/bitbots_hcm/CMakeLists.txt +++ b/bitbots_motion/bitbots_hcm/CMakeLists.txt @@ -57,6 +57,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 8f805b1da..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): @@ -231,6 +234,10 @@ def start_animation(self): goal = Dynup.Goal() goal.direction = self.direction + # This indicates that the goal is send from the hcm and therfore has prio, + # canceling other tasks. The published joint goals are also marked with this flag so they + # are handled differently in the HCM joint mutex + goal.from_hcm = True self.blackboard.dynup_action_current_goal = self.blackboard.dynup_action_client.send_goal_async( goal, feedback_callback=self.animation_feedback_cb ) @@ -241,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_hcm/src/hcm.cpp b/bitbots_motion/bitbots_hcm/src/hcm.cpp index d82358c0d..c78574c32 100644 --- a/bitbots_motion/bitbots_hcm/src/hcm.cpp +++ b/bitbots_motion/bitbots_hcm/src/hcm.cpp @@ -86,12 +86,15 @@ class HCM_CPP : public rclcpp::Node { } void dynup_callback(const bitbots_msgs::msg::JointCommand msg) { - if (current_state_ == bitbots_msgs::msg::RobotControlState::STARTUP || - current_state_ == bitbots_msgs::msg::RobotControlState::GETTING_UP || - current_state_ == bitbots_msgs::msg::RobotControlState::MOTOR_OFF || - current_state_ == bitbots_msgs::msg::RobotControlState::PICKED_UP || - current_state_ == bitbots_msgs::msg::RobotControlState::PENALTY || - current_state_ == bitbots_msgs::msg::RobotControlState::RECORD || + if (msg.from_hcm and (current_state_ == bitbots_msgs::msg::RobotControlState::STARTUP || + current_state_ == bitbots_msgs::msg::RobotControlState::GETTING_UP || + current_state_ == bitbots_msgs::msg::RobotControlState::MOTOR_OFF || + current_state_ == bitbots_msgs::msg::RobotControlState::PICKED_UP || + current_state_ == bitbots_msgs::msg::RobotControlState::PENALTY || + current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE)) { + pub_controller_command_->publish(msg); + } + if (current_state_ == bitbots_msgs::msg::RobotControlState::RECORD || current_state_ == bitbots_msgs::msg::RobotControlState::CONTROLLABLE) { pub_controller_command_->publish(msg); } 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_msgs/action/Dynup.action b/bitbots_msgs/action/Dynup.action index 6e5620f2f..939261314 100644 --- a/bitbots_msgs/action/Dynup.action +++ b/bitbots_msgs/action/Dynup.action @@ -9,6 +9,9 @@ string DIRECTION_RISE = "rise" string DIRECTION_DESCEND = "descend" string DIRECTION_WALKREADY = "walkready" +# Requests from the HCM have higher prio and are published to a different topic +bool from_hcm + --- # Result definition bool successful diff --git a/bitbots_msgs/msg/JointCommand.msg b/bitbots_msgs/msg/JointCommand.msg index 115e0db78..c86b230dc 100644 --- a/bitbots_msgs/msg/JointCommand.msg +++ b/bitbots_msgs/msg/JointCommand.msg @@ -1,4 +1,8 @@ std_msgs/Header header + +# Mark this message so it has prio +bool from_hcm + string[] joint_names float64[] positions diff --git a/bitbots_navigation/bitbots_localization/config/config.yaml b/bitbots_navigation/bitbots_localization/config/config.yaml index 22d0e5b52..13e135e94 100644 --- a/bitbots_navigation/bitbots_localization/config/config.yaml +++ b/bitbots_navigation/bitbots_localization/config/config.yaml @@ -3,15 +3,11 @@ bitbots_localization: misc: init_mode: 0 percentage_best_particles: 100 - min_motion_linear: 0.0 - min_motion_angular: 0.0 max_motion_linear: 0.5 max_motion_angular: 3.14 - filter_only_with_motion: false ros: line_pointcloud_topic: 'line_mask_relative_pc' goal_topic: 'goals_simulated' - fieldboundary_topic: 'field_boundary_relative' particle_publishing_topic: 'pose_particles' debug_visualization: true particle_filter: @@ -46,10 +42,6 @@ bitbots_localization: goal: factor: 0.0 out_of_field_score: 0.0 - field_boundary: - factor: 0.0 - out_of_field_score: 0.0 confidences: line_element: 0.01 goal_element: 0.0 - field_boundary_element: 0.0 diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp index 50631ad56..30ecaf993 100644 --- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp +++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/ObservationModel.hpp @@ -5,6 +5,8 @@ #ifndef BITBOTS_LOCALIZATION_OBSERVATIONMODEL_H #define BITBOTS_LOCALIZATION_OBSERVATIONMODEL_H +#include + #include #include #include @@ -12,11 +14,8 @@ #include #include #include -#include #include #include -#include -#include namespace sm = sensor_msgs; namespace bl = bitbots_localization; @@ -29,8 +28,7 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel map_lines, std::shared_ptr map_goals, - std::shared_ptr map_field_boundary, const bitbots_localization::Params &config, - const FieldDimensions &field_dimensions); + const bitbots_localization::Params &config, const FieldDimensions &field_dimensions); /** * @@ -43,15 +41,9 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel> get_measurement_lines() const; + const std::vector> get_measurement_lines() const; - std::vector> get_measurement_goals() const; - - std::vector> get_measurement_field_boundary() const; + const std::vector> get_measurement_goals() const; double get_min_weight() const override; @@ -59,20 +51,26 @@ class RobotPoseObservationModel : public particle_filter::ObservationModel> &last_measurement, - std::shared_ptr map, double element_weight) const; + std::shared_ptr map, double element_weight, + const tf2::Transform &movement_since_measurement) const; // Measurements std::vector> last_measurement_lines_; std::vector> last_measurement_goal_; - std::vector> last_measurement_field_boundary_; + + // Movement since last measurement + tf2::Transform movement_since_line_measurement_ = tf2::Transform::getIdentity(); + tf2::Transform movement_since_goal_measurement_ = tf2::Transform::getIdentity(); // Reference to the maps for the different classes std::shared_ptr map_lines_; std::shared_ptr map_goals_; - std::shared_ptr map_field_boundary_; // Parameters bitbots_localization::Params config_; diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/RobotState.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/RobotState.hpp index 34391a1ac..d457b09cd 100644 --- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/RobotState.hpp +++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/RobotState.hpp @@ -10,6 +10,8 @@ #include #include #include +#include +#include #include namespace bitbots_localization { @@ -24,10 +26,17 @@ class RobotState { /** * @param x Position of the robot. * @param y Position of the robot. - * @param T Orientaion of the robot in radians. + * @param T Orientation of the robot in radians. */ RobotState(double x, double y, double T); + /** + * @brief Constructor for a robot state based on a tf2::Transform. + * + * @param transform Transform of the robots base_footprint in the map frame. + */ + explicit RobotState(tf2::Transform transform); + RobotState operator*(float factor) const; RobotState &operator+=(const RobotState &other); @@ -62,6 +71,8 @@ class RobotState { visualization_msgs::msg::Marker renderMarker(std::string n_space, std::string frame, rclcpp::Duration lifetime, std_msgs::msg::ColorRGBA color, rclcpp::Time stamp) const; + tf2::Transform getTransform() const; + private: double m_XPos; double m_YPos; diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp index 643f3147b..95f3521a1 100644 --- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp +++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/localization.hpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include #include @@ -109,12 +108,6 @@ class Localization { */ void GoalPostsCallback(const sv3dm::msg::GoalpostArray &msg); // TODO - /** - * Callback for the relative field boundary measurements - * @param msg Message containing the field boundary points. - */ - void FieldboundaryCallback(const sv3dm::msg::FieldBoundary &msg); - /** * Resets the state distribution of the state space * @param distribution The type of the distribution @@ -157,17 +150,13 @@ class Localization { // Declare subscribers rclcpp::Subscription::SharedPtr line_point_cloud_subscriber_; rclcpp::Subscription::SharedPtr goal_subscriber_; - rclcpp::Subscription::SharedPtr fieldboundary_subscriber_; - rclcpp::Subscription::SharedPtr rviz_initial_pose_subscriber_; // Declare publishers rclcpp::Publisher::SharedPtr pose_with_covariance_publisher_; rclcpp::Publisher::SharedPtr pose_particles_publisher_; - rclcpp::Publisher::SharedPtr lines_publisher_; rclcpp::Publisher::SharedPtr line_ratings_publisher_; rclcpp::Publisher::SharedPtr goal_ratings_publisher_; - rclcpp::Publisher::SharedPtr fieldboundary_ratings_publisher_; rclcpp::Publisher::SharedPtr field_publisher_; // Declare services @@ -178,8 +167,8 @@ class Localization { rclcpp::TimerBase::SharedPtr publishing_timer_; // Declare tf2 objects - std::unique_ptr tfBuffer; - std::shared_ptr tfListener; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; std::shared_ptr br; // Declare particle filter components @@ -200,7 +189,6 @@ class Localization { // Declare input message buffers sm::msg::PointCloud2 line_pointcloud_relative_; sv3dm::msg::GoalpostArray goal_posts_relative_; - sv3dm::msg::FieldBoundary fieldboundary_relative_; // Declare time stamps rclcpp::Time last_stamp_lines = rclcpp::Time(0); @@ -218,16 +206,12 @@ class Localization { // Keep track of the odometry transform in the last step geometry_msgs::msg::TransformStamped previousOdomTransform_; - // Flag that checks if the robot is moving - bool robot_moved = false; - // Keep track of the number of filter steps int timer_callback_count_ = 0; // Maps for the different measurement classes std::shared_ptr lines_; std::shared_ptr goals_; - std::shared_ptr field_boundary_; // RNG that is used for the different sampling steps particle_filter::CRandomNumberGenerator random_number_generator_; @@ -270,8 +254,8 @@ class Localization { * @param map map for this class * @param publisher ros publisher for the type visualization_msgs::msg::Marker */ - void publish_debug_rating(std::vector> measurements, double scale, const char name[24], - std::shared_ptr map, + void publish_debug_rating(const std::vector> &measurements, double scale, + const char name[24], std::shared_ptr map, rclcpp::Publisher::SharedPtr &publisher); /** diff --git a/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp b/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp index 44839346d..bee35e365 100644 --- a/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp +++ b/bitbots_navigation/bitbots_localization/include/bitbots_localization/map.hpp @@ -45,8 +45,8 @@ class Map { double get_occupancy(double x, double y); - std::pair observationRelative(std::pair observation, double stateX, double stateY, - double stateT); + std::pair getObservationCoordinatesInMapFrame(std::pair observation, double stateX, + double stateY, double stateT); nav_msgs::msg::OccupancyGrid get_map_msg(std::string frame_id, int threshold = -1); diff --git a/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp b/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp index 4f68664d0..d3f4c4fa5 100644 --- a/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp +++ b/bitbots_navigation/bitbots_localization/src/ObservationModel.cpp @@ -7,13 +7,11 @@ namespace bitbots_localization { RobotPoseObservationModel::RobotPoseObservationModel(std::shared_ptr map_lines, std::shared_ptr map_goals, - std::shared_ptr map_field_boundary, const bitbots_localization::Params &config, const FieldDimensions &field_dimensions) : particle_filter::ObservationModel(), map_lines_(map_lines), map_goals_(map_goals), - map_field_boundary_(map_field_boundary), config_(config), field_dimensions_(field_dimensions) { particle_filter::ObservationModel::accumulate_weights_ = true; @@ -21,10 +19,13 @@ RobotPoseObservationModel::RobotPoseObservationModel(std::shared_ptr map_li double RobotPoseObservationModel::calculate_weight_for_class( const RobotState &state, const std::vector> &last_measurement, std::shared_ptr map, - double element_weight) const { + double element_weight, const tf2::Transform &movement_since_measurement) const { double particle_weight_for_class; if (!last_measurement.empty()) { - std::vector ratings = map->Map::provideRating(state, last_measurement); + // Subtract (reverse) the movement from our state to get the hypothetical state at the time of the measurement + const RobotState state_at_measurement(state.getTransform() * movement_since_measurement.inverse()); + // Get the ratings for for all points in the measurement based on the map + const std::vector ratings = map->Map::provideRating(state_at_measurement, last_measurement); // Take the average of the ratings (functional) particle_weight_for_class = std::pow(std::accumulate(ratings.begin(), ratings.end(), 0.0) / ratings.size(), 2); } else { @@ -34,22 +35,19 @@ double RobotPoseObservationModel::calculate_weight_for_class( } double RobotPoseObservationModel::measure(const RobotState &state) const { - double particle_weight_lines = calculate_weight_for_class(state, last_measurement_lines_, map_lines_, - config_.particle_filter.confidences.line_element); - double particle_weight_goal = calculate_weight_for_class(state, last_measurement_goal_, map_goals_, - config_.particle_filter.confidences.goal_element); - double particle_weight_field_boundary = - calculate_weight_for_class(state, last_measurement_field_boundary_, map_field_boundary_, - config_.particle_filter.confidences.field_boundary_element); + double particle_weight_lines = + calculate_weight_for_class(state, last_measurement_lines_, map_lines_, + config_.particle_filter.confidences.line_element, movement_since_line_measurement_); + double particle_weight_goal = + calculate_weight_for_class(state, last_measurement_goal_, map_goals_, + config_.particle_filter.confidences.goal_element, movement_since_goal_measurement_); // Get relevant config values auto scoring_config = config_.particle_filter.scoring; // Calculate weight for the particle double weight = (((1 - scoring_config.lines.factor) + scoring_config.lines.factor * particle_weight_lines) * - ((1 - scoring_config.goal.factor) + scoring_config.goal.factor * particle_weight_goal) * - ((1 - scoring_config.field_boundary.factor) + - scoring_config.field_boundary.factor * particle_weight_field_boundary)); + ((1 - scoring_config.goal.factor) + scoring_config.goal.factor * particle_weight_goal)); if (weight < config_.particle_filter.weighting.min_weight) { weight = config_.particle_filter.weighting.min_weight; @@ -68,6 +66,7 @@ double RobotPoseObservationModel::measure(const RobotState &state) const { } void RobotPoseObservationModel::set_measurement_lines_pc(sm::msg::PointCloud2 measurement) { + // convert to polar for (sm::PointCloud2ConstIterator iter_xyz(measurement, "x"); iter_xyz != iter_xyz.end(); ++iter_xyz) { std::pair linePolar = cartesianToPolar(iter_xyz[0], iter_xyz[1]); last_measurement_lines_.push_back(linePolar); @@ -82,39 +81,33 @@ void RobotPoseObservationModel::set_measurement_goalposts(sv3dm::msg::GoalpostAr } } -void RobotPoseObservationModel::set_measurement_field_boundary(sv3dm::msg::FieldBoundary measurement) { - // convert to polar - for (gm::msg::Point &point : measurement.points) { - std::pair fieldBoundaryPointPolar = cartesianToPolar(point.x, point.y); - last_measurement_field_boundary_.push_back(fieldBoundaryPointPolar); - } -} - -std::vector> RobotPoseObservationModel::get_measurement_lines() const { +const std::vector> RobotPoseObservationModel::get_measurement_lines() const { return last_measurement_lines_; } -std::vector> RobotPoseObservationModel::get_measurement_goals() const { +const std::vector> RobotPoseObservationModel::get_measurement_goals() const { return last_measurement_goal_; } -std::vector> RobotPoseObservationModel::get_measurement_field_boundary() const { - return last_measurement_field_boundary_; -} - double RobotPoseObservationModel::get_min_weight() const { return config_.particle_filter.weighting.min_weight; } void RobotPoseObservationModel::clear_measurement() { last_measurement_lines_.clear(); last_measurement_goal_.clear(); - last_measurement_field_boundary_.clear(); } bool RobotPoseObservationModel::measurements_available() { bool available = false; available |= !last_measurement_lines_.empty(); available |= !last_measurement_goal_.empty(); - available |= !last_measurement_field_boundary_.empty(); return available; } + +void RobotPoseObservationModel::set_movement_since_line_measurement(const tf2::Transform movement) { + movement_since_line_measurement_ = movement; +} + +void RobotPoseObservationModel::set_movement_since_goal_measurement(const tf2::Transform movement) { + movement_since_goal_measurement_ = movement; +} } // namespace bitbots_localization diff --git a/bitbots_navigation/bitbots_localization/src/RobotState.cpp b/bitbots_navigation/bitbots_localization/src/RobotState.cpp index b36ad47f3..3f1cde1e2 100644 --- a/bitbots_navigation/bitbots_localization/src/RobotState.cpp +++ b/bitbots_navigation/bitbots_localization/src/RobotState.cpp @@ -11,6 +11,11 @@ RobotState::RobotState() : is_explorer_(false), m_XPos(0.0), m_YPos(0.0), m_SinT RobotState::RobotState(double x, double y, double T) : is_explorer_(false), m_XPos(x), m_YPos(y), m_SinTheta(sin(T)), m_CosTheta(cos(T)) {} +RobotState::RobotState(tf2::Transform transform) + : is_explorer_(false), m_XPos(transform.getOrigin().x()), m_YPos(transform.getOrigin().y()) { + setTheta(tf2::getYaw(transform.getRotation())); +} + RobotState RobotState::operator*(float factor) const { RobotState newState; newState.m_XPos = m_XPos * factor; @@ -119,4 +124,14 @@ visualization_msgs::msg::Marker RobotState::renderMarker(std::string n_space, st return msg; } + +tf2::Transform RobotState::getTransform() const { + tf2::Transform transform; + transform.setOrigin(tf2::Vector3(getXPos(), getYPos(), 0)); + tf2::Quaternion q; + q.setRPY(0, 0, getTheta()); + transform.setRotation(q); + return transform; +} + } // namespace bitbots_localization diff --git a/bitbots_navigation/bitbots_localization/src/localization.cpp b/bitbots_navigation/bitbots_localization/src/localization.cpp index c3cd2f8bd..89c2e1a92 100644 --- a/bitbots_navigation/bitbots_localization/src/localization.cpp +++ b/bitbots_navigation/bitbots_localization/src/localization.cpp @@ -8,16 +8,16 @@ Localization::Localization(std::shared_ptr node) : node_(node), param_listener_(node->get_node_parameters_interface()), config_(param_listener_.get_params()), - tfBuffer(std::make_unique(node->get_clock())), - tfListener(std::make_shared(*tfBuffer, node)), + tf_buffer_(std::make_shared(node->get_clock())), + tf_listener_(std::make_shared(*tf_buffer_, node)), br(std::make_shared(node)) { // Wait for transforms to become available and init them - bitbots_utils::wait_for_tf(node->get_logger(), node->get_clock(), tfBuffer.get(), {config_.ros.base_footprint_frame}, - config_.ros.odom_frame); + bitbots_utils::wait_for_tf(node->get_logger(), node->get_clock(), tf_buffer_.get(), + {config_.ros.base_footprint_frame}, config_.ros.odom_frame); // Get the initial transform from odom to base_footprint - previousOdomTransform_ = tfBuffer->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, - rclcpp::Time(0), rclcpp::Duration::from_nanoseconds(1e9 * 20.0)); + previousOdomTransform_ = tf_buffer_->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, + rclcpp::Time(0), rclcpp::Duration::from_nanoseconds(1e9 * 20.0)); // Init subscribers line_point_cloud_subscriber_ = node->create_subscription( @@ -26,9 +26,6 @@ Localization::Localization(std::shared_ptr node) goal_subscriber_ = node->create_subscription( config_.ros.goal_topic, 1, std::bind(&Localization::GoalPostsCallback, this, _1)); - fieldboundary_subscriber_ = node->create_subscription( - config_.ros.fieldboundary_topic, 1, std::bind(&Localization::FieldboundaryCallback, this, _1)); - rviz_initial_pose_subscriber_ = node->create_subscription( "initialpose", 1, std::bind(&Localization::SetInitialPositionCallback, this, _1)); @@ -37,18 +34,14 @@ Localization::Localization(std::shared_ptr node) node->create_publisher(config_.ros.particle_publishing_topic, 1); pose_with_covariance_publisher_ = node->create_publisher("pose_with_covariance", 1); - lines_publisher_ = node->create_publisher("lines", 1); line_ratings_publisher_ = node->create_publisher("line_ratings", 1); goal_ratings_publisher_ = node->create_publisher("goal_ratings", 1); - fieldboundary_ratings_publisher_ = - node->create_publisher("field_boundary_ratings", 1); field_publisher_ = node->create_publisher( "field/map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local()); // Set the measurement timestamps to 0 line_pointcloud_relative_.header.stamp = rclcpp::Time(0); goal_posts_relative_.header.stamp = rclcpp::Time(0); - fieldboundary_relative_.header.stamp = rclcpp::Time(0); // Get the static global configuration from the blackboard auto global_params = bitbots_utils::get_parameters_from_other_node( @@ -99,14 +92,9 @@ void Localization::updateParams(bool force_reload) { if (config_.particle_filter.scoring.goal.factor) { goals_.reset(new Map(field_name_, "goals.png", config_.particle_filter.scoring.goal.out_of_field_score)); } - if (config_.particle_filter.scoring.field_boundary.factor) { - field_boundary_.reset( - new Map(field_name_, "field_boundary.png", config_.particle_filter.scoring.field_boundary.out_of_field_score)); - } // Init observation model - robot_pose_observation_model_.reset( - new RobotPoseObservationModel(lines_, goals_, field_boundary_, config_, field_dimensions_)); + robot_pose_observation_model_.reset(new RobotPoseObservationModel(lines_, goals_, config_, field_dimensions_)); // Init motion model auto drift_config = config_.particle_filter.drift; @@ -174,10 +162,9 @@ void Localization::run_filter_one_step() { robot_motion_model_->diffuse_multiplier_ = config_.particle_filter.diffusion.multiplier; } - if ((config_.misc.filter_only_with_motion and robot_moved) or (!config_.misc.filter_only_with_motion)) { - robot_pf_->drift(linear_movement_, rotational_movement_); - robot_pf_->diffuse(); - } + // Apply the motion model to the particles + robot_pf_->drift(linear_movement_, rotational_movement_); + robot_pf_->diffuse(); // Apply ratings corresponding to the observations compared with each particle position robot_pf_->measure(); @@ -202,11 +189,9 @@ void Localization::LinePointcloudCallback(const sm::msg::PointCloud2 &msg) { lin void Localization::GoalPostsCallback(const sv3dm::msg::GoalpostArray &msg) { goal_posts_relative_ = msg; } -void Localization::FieldboundaryCallback(const sv3dm::msg::FieldBoundary &msg) { fieldboundary_relative_ = msg; } - void Localization::SetInitialPositionCallback(const gm::msg::PoseWithCovarianceStamped &msg) { // Transform the given pose to map frame - auto pose_in_map = tfBuffer->transform(msg, config_.ros.map_frame, tf2::durationFromSec(1.0)); + auto pose_in_map = tf_buffer_->transform(msg, config_.ros.map_frame, tf2::durationFromSec(1.0)); // Get yaw from quaternion double yaw = tf2::getYaw(pose_in_map.pose.pose.orientation); @@ -289,22 +274,42 @@ void Localization::reset_filter(int distribution, double x, double y, double ang } void Localization::updateMeasurements() { + // Define an inner lambda function to calculate the odometry movement since a given timestamp + auto movement_since_stamp = [this](rclcpp::Time stamp) { + try { + tf2::Transform odom_at_measurement; + // Get the transform from the time the measurement was taken and copy the transform to a tf2 transform + tf2::fromMsg( + tf_buffer_->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, stamp).transform, + odom_at_measurement); + tf2::Transform odom_now; + // Get the latest transform and copy the transform to a tf2 transform + tf2::fromMsg( + tf_buffer_->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0)) + .transform, + odom_now); + // Calculate the movement since the measurement was taken (in the local frame) + return odom_at_measurement.inverseTimes(odom_now); + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN(node_->get_logger(), "Could not acquire movement since measurement at time: %s Assumed no movement.", + ex.what()); + return tf2::Transform::getIdentity(); + } + }; + // Sets the measurements in the observation model if (line_pointcloud_relative_.header.stamp != last_stamp_lines && config_.particle_filter.scoring.lines.factor) { robot_pose_observation_model_->set_measurement_lines_pc(line_pointcloud_relative_); + robot_pose_observation_model_->set_movement_since_line_measurement( + movement_since_stamp(line_pointcloud_relative_.header.stamp)); + last_stamp_lines = line_pointcloud_relative_.header.stamp; } if (config_.particle_filter.scoring.goal.factor && goal_posts_relative_.header.stamp != last_stamp_goals) { robot_pose_observation_model_->set_measurement_goalposts(goal_posts_relative_); + robot_pose_observation_model_->set_movement_since_goal_measurement( + movement_since_stamp(goal_posts_relative_.header.stamp)); + last_stamp_goals = goal_posts_relative_.header.stamp; } - if (config_.particle_filter.scoring.field_boundary.factor && - fieldboundary_relative_.header.stamp != last_stamp_fb_points) { - robot_pose_observation_model_->set_measurement_field_boundary(fieldboundary_relative_); - } - - // Set timestamps to mark past messages - last_stamp_lines = line_pointcloud_relative_.header.stamp; - last_stamp_goals = goal_posts_relative_.header.stamp; - last_stamp_fb_points = fieldboundary_relative_.header.stamp; } void Localization::getMotion() { @@ -313,26 +318,19 @@ void Localization::getMotion() { try { // Get current odometry transform transformStampedNow = - tfBuffer->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0)); - - // Get linear movement from odometry transform and the transform of the previous filter step - double global_diff_x, global_diff_y; - global_diff_x = (transformStampedNow.transform.translation.x - previousOdomTransform_.transform.translation.x); - global_diff_y = (transformStampedNow.transform.translation.y - previousOdomTransform_.transform.translation.y); - - // Convert to local frame - auto [polar_rot, polar_dist] = cartesianToPolar(global_diff_x, global_diff_y); - auto [local_movement_x, local_movement_y] = - polarToCartesian(polar_rot - tf2::getYaw(previousOdomTransform_.transform.rotation), polar_dist); - linear_movement_.x = local_movement_x; - linear_movement_.y = local_movement_y; - linear_movement_.z = 0; - - // Get angular movement from odometry transform and the transform of the previous filter step - rotational_movement_.x = 0; - rotational_movement_.y = 0; - rotational_movement_.z = - tf2::getYaw(transformStampedNow.transform.rotation) - tf2::getYaw(previousOdomTransform_.transform.rotation); + tf_buffer_->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0)); + + // Convert Msgs to tf2 transforms that support mathematical operations + tf2::Transform transformNow, transformPreviousOdom; + tf2::fromMsg(transformStampedNow.transform, transformNow); + tf2::fromMsg(previousOdomTransform_.transform, transformPreviousOdom); + + // Calculate the movement in the local frame + auto movement = transformPreviousOdom.inverseTimes(transformNow); + + // Copy the movement to the message to comply with the api, which expects messages and not tf2 transforms + tf2::convert(movement.getOrigin(), linear_movement_); + rotational_movement_.z = tf2::getYaw(movement.getRotation()); // Get the time delta between the two transforms double time_delta = rclcpp::Time(transformStampedNow.header.stamp).seconds() - @@ -354,15 +352,9 @@ void Localization::getMotion() { linear_movement_.y = 0; RCLCPP_WARN(node_->get_logger(), "Robot moved an unreasonable amount, dropping motion."); } - - // Check if robot moved - robot_moved = linear_movement_normalized_x >= config_.misc.min_motion_linear or - linear_movement_normalized_y >= config_.misc.min_motion_linear or - rotational_movement_normalized_z >= config_.misc.min_motion_angular; } else { RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, "Time step delta of zero encountered! Odometry is unavailable."); - robot_moved = false; } // Set the variable for the transform of the previous step to the transform of the current step, because we finished @@ -387,7 +379,7 @@ void Localization::publish_transforms() { // Get the transform from the last measurement timestamp until now geometry_msgs::msg::TransformStamped odomDuringMeasurement, odomNow; try { - odomNow = tfBuffer->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0)); + odomNow = tf_buffer_->lookupTransform(config_.ros.odom_frame, config_.ros.base_footprint_frame, rclcpp::Time(0)); } catch (const tf2::TransformException &ex) { RCLCPP_WARN(node_->get_logger(), "Could not acquire odom transforms: %s", ex.what()); } @@ -508,14 +500,9 @@ void Localization::publish_ratings() { publish_debug_rating(robot_pose_observation_model_->get_measurement_goals(), 0.2, "goal_ratings", goals_, goal_ratings_publisher_); } - if (config_.particle_filter.scoring.field_boundary.factor) { - // Publish field boundary ratings - publish_debug_rating(robot_pose_observation_model_->get_measurement_field_boundary(), 0.2, "field_boundary_ratings", - field_boundary_, fieldboundary_ratings_publisher_); - } } -void Localization::publish_debug_rating(std::vector> measurements, double scale, +void Localization::publish_debug_rating(const std::vector> &measurements, double scale, const char name[], std::shared_ptr map, rclcpp::Publisher::SharedPtr &publisher) { RobotState best_estimate = robot_pf_->getBestXPercentEstimate(config_.misc.percentage_best_particles); @@ -530,18 +517,17 @@ void Localization::publish_debug_rating(std::vector> m marker.scale.x = scale; marker.scale.y = scale; - for (std::pair &measurement : measurements) { + for (const std::pair &measurement : measurements) { // lines are in polar form! std::pair observationRelative; - observationRelative = map->observationRelative(measurement, best_estimate.getXPos(), best_estimate.getYPos(), - best_estimate.getTheta()); + observationRelative = map->getObservationCoordinatesInMapFrame(measurement, best_estimate.getXPos(), + best_estimate.getYPos(), best_estimate.getTheta()); double occupancy = map->get_occupancy(observationRelative.first, observationRelative.second); geometry_msgs::msg::Point point; point.x = observationRelative.first; point.y = observationRelative.second; - point.z = 0; std_msgs::msg::ColorRGBA color; color.b = 1; diff --git a/bitbots_navigation/bitbots_localization/src/map.cpp b/bitbots_navigation/bitbots_localization/src/map.cpp index dc63179cb..6491368c4 100644 --- a/bitbots_navigation/bitbots_localization/src/map.cpp +++ b/bitbots_navigation/bitbots_localization/src/map.cpp @@ -56,7 +56,7 @@ std::vector Map::provideRating(const RobotState &state, std::pair lineRelative; // get rating per line - lineRelative = observationRelative(observation, state.getXPos(), state.getYPos(), state.getTheta()); + lineRelative = getObservationCoordinatesInMapFrame(observation, state.getXPos(), state.getYPos(), state.getTheta()); double occupancy = get_occupancy(lineRelative.first, lineRelative.second); rating.push_back(occupancy); @@ -64,12 +64,12 @@ std::vector Map::provideRating(const RobotState &state, return rating; } -std::pair Map::observationRelative( - std::pair observation, double stateX, double stateY, - double stateT) { // todo rename to a more correct name like observationonmap? - // transformes observation from particle to map (assumes particle is correct) - // input: obsservation relative in polar coordinates and particle - // output: hypothetical observation on map +std::pair Map::getObservationCoordinatesInMapFrame(std::pair observation, double stateX, + double stateY, double stateT) { + // queries the Cartesian metric map coordinates for a given observation (in polar coordinates) + // taken relative to a given state (in Cartesian coordinates) + // Input: Observation coordinates in polar coordinates, state coordinates in Cartesian coordinates + // Output: Observation coordinates in Cartesian coordinates in the map frame // add theta and convert back to cartesian std::pair observationWithTheta = polarToCartesian(observation.first + stateT, observation.second); @@ -78,14 +78,6 @@ std::pair Map::observationRelative( std::pair observationRelative = std::make_pair(stateX + observationWithTheta.first, stateY + observationWithTheta.second); - // alternativ: - // Thrun 6.32, seite 169 - // but both equivalent - // double xGlobal = stateX + observation.second * (cos(stateT + observation.first)); - // double yGlobal = stateY + observation.second * (sin(stateT + observation.first)); - - // std::pair observationRelative = std::make_pair(xGlobal, yGlobal); - return observationRelative; // in cartesian } diff --git a/bitbots_navigation/bitbots_localization/src/parameters.yml b/bitbots_navigation/bitbots_localization/src/parameters.yml index ad4f6fcac..610a55754 100644 --- a/bitbots_navigation/bitbots_localization/src/parameters.yml +++ b/bitbots_navigation/bitbots_localization/src/parameters.yml @@ -10,29 +10,16 @@ bitbots_localization: description: "Percentage of particles which are considered to be the best ones. These particles are used to calculate the pose estimate" validation: bounds<>: [0, 100] - min_motion_linear: - type: double - description: "Minimum linear motion (m/s) which is considered to be a movement. This is relevant if we want to deactivate the filter if no movement is detected" - validation: - gt_eq<>: [0.0] max_motion_linear: type: double description: "Maximum linear motion (m/s) which is considered to be a movement. Updates larger than this are deemed unrealistic and are ignored" validation: gt_eq<>: [0.0] - min_motion_angular: - type: double - description: "Minimum angular motion (rad/s) which is considered to be a movement. This is relevant if we want to deactivate the filter if no movement is detected" - validation: - gt_eq<>: [0.0] max_motion_angular: type: double description: "Maximum angular motion (rad/s) which is considered to be a movement. Updates larger than this are deemed unrealistic and are ignored" validation: gt_eq<>: [0.0] - filter_only_with_motion: - type: bool - description: "If true, the filter is only active if a movement is detected" ros: line_pointcloud_topic: type: string @@ -42,10 +29,6 @@ bitbots_localization: type: string description: "Topic for the goal input messages" read_only: true - fieldboundary_topic: - type: string - description: "Topic for the field boundary input messages" - read_only: true particle_publishing_topic: type: string description: "Topic for the particle publishing messages" @@ -203,17 +186,6 @@ bitbots_localization: description: "Score which is given to a measurement (e.g. projected goal post) if it is out of the field" validation: bounds<>: [0.0, 100.0] - field_boundary: - factor: - type: double - description: "Weighing how much the field boundary information is considered for the scoring of a particle" - validation: - bounds<>: [0.0, 1.0] - out_of_field_score: - type: double - description: "Score which is given to a measurement (e.g. projected field boundary segment) if it is out of the field" - validation: - bounds<>: [0.0, 100.0] confidences: line_element: type: double @@ -225,8 +197,3 @@ bitbots_localization: description: "Confidence we have in each data point of the goal information. Meaning each projected goal post" validation: bounds<>: [0.0, 1.0] - field_boundary_element: - type: double - description: "Confidence we have in each data point of the field boundary information. Meaning each projected field boundary segment" - validation: - bounds<>: [0.0, 1.0] 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 4600ee5c2..2d1f97edd 100644 --- a/bitbots_simulation/bitbots_webots_sim/CMakeLists.txt +++ b/bitbots_simulation/bitbots_webots_sim/CMakeLists.txt @@ -83,6 +83,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 57ca74e24..972bf4400 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 529c50300..ec97049c4 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 python3-pytest 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 65fc7a504..c8298277f 100644 --- a/bitbots_team_communication/bitbots_team_data_sim_rqt/setup.py +++ b/bitbots_team_communication/bitbots_team_data_sim_rqt/setup.py @@ -14,6 +14,7 @@ install_requires=["setuptools"], tests_require=["pytest"], 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 752e2fc74..6f5b75f5e 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 python3-pytest 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 96b053d04..6c563954c 100755 --- a/bitbots_vision/setup.py +++ b/bitbots_vision/setup.py @@ -27,6 +27,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 554e655e3..73df18f0f 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 python3-pytest diff --git a/bitbots_world_model/bitbots_ball_filter/setup.py b/bitbots_world_model/bitbots_ball_filter/setup.py index c98cbb296..4080aaa35 100644 --- a/bitbots_world_model/bitbots_ball_filter/setup.py +++ b/bitbots_world_model/bitbots_ball_filter/setup.py @@ -28,6 +28,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 diff --git a/workspace.repos b/workspace.repos index 31eb2c056..6b0e3aa45 100644 --- a/workspace.repos +++ b/workspace.repos @@ -15,6 +15,10 @@ repositories: type: git url: git@github.com:bit-bots/bio_ik_msgs.git version: master + lib/bitbots_tf_buffer: + type: git + url: git@github.com:bit-bots/bitbots_tf_buffer.git + version: main lib/biped_interfaces: type: git url: git@github.com:ros-sports/biped_interfaces.git