diff --git a/.vscode/settings.json b/.vscode/settings.json index a88bad372..f16e3fffc 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -16,6 +16,7 @@ "colcon", "cornerkick", "costmap", + "costmaps", "cpus", "cuda", "cudnn", @@ -27,6 +28,7 @@ "dtype", "dynamixel", "dynup", + "euler", "externproto", "fetchrobotics", "fieldboundary", @@ -35,6 +37,7 @@ "freekick", "gamecontroller", "gamestate", + "gaussian", "goalkick", "hexsha", "hlvs", @@ -63,6 +66,7 @@ "odom", "odometry", "particlefilter", + "pathfinding", "penaltykick", "penaltyshoot", "pointcloud", @@ -209,4 +213,7 @@ "editor.formatOnSave": true, "editor.defaultFormatter": "charliermarsh.ruff", "restructuredtext.pythonRecommendation.disabled": true, + "[xml]": { + "editor.defaultFormatter": "DotJoshJohnson.xml" + }, } diff --git a/bitbots_behavior/bitbots_blackboard/.idea/.gitignore b/bitbots_behavior/bitbots_blackboard/.idea/.gitignore deleted file mode 100644 index 26d33521a..000000000 --- a/bitbots_behavior/bitbots_blackboard/.idea/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -# Default ignored files -/shelf/ -/workspace.xml diff --git a/bitbots_behavior/bitbots_blackboard/.idea/bitbots_blackboard.iml b/bitbots_behavior/bitbots_blackboard/.idea/bitbots_blackboard.iml deleted file mode 100644 index 8e5446ac9..000000000 --- a/bitbots_behavior/bitbots_blackboard/.idea/bitbots_blackboard.iml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - \ No newline at end of file diff --git a/bitbots_behavior/bitbots_blackboard/.idea/inspectionProfiles/profiles_settings.xml b/bitbots_behavior/bitbots_blackboard/.idea/inspectionProfiles/profiles_settings.xml deleted file mode 100644 index 105ce2da2..000000000 --- a/bitbots_behavior/bitbots_blackboard/.idea/inspectionProfiles/profiles_settings.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/bitbots_behavior/bitbots_blackboard/.idea/misc.xml b/bitbots_behavior/bitbots_blackboard/.idea/misc.xml deleted file mode 100644 index 457547d70..000000000 --- a/bitbots_behavior/bitbots_blackboard/.idea/misc.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ No newline at end of file diff --git a/bitbots_behavior/bitbots_blackboard/.idea/modules.xml b/bitbots_behavior/bitbots_blackboard/.idea/modules.xml deleted file mode 100644 index ec63261bd..000000000 --- a/bitbots_behavior/bitbots_blackboard/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/bitbots_behavior/bitbots_blackboard/.idea/vcs.xml b/bitbots_behavior/bitbots_blackboard/.idea/vcs.xml deleted file mode 100644 index 6c0b86358..000000000 --- a/bitbots_behavior/bitbots_blackboard/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py index c996a2674..1f9dcfa64 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py @@ -1,9 +1,3 @@ -""" -AnimationCapsule -^^^^^^^^^^^^^^^^ - -Communicates with the animation action server and plays predefined animations. -""" from rclpy.action import ActionClient from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.duration import Duration @@ -13,6 +7,8 @@ class AnimationCapsule: + """Communicates with the animation action server to play animations.""" + def __init__(self, node: Node): self.node = node self.active = False 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 5c0a50d29..d969c6f12 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py @@ -1,9 +1,3 @@ -""" -CostmapCapsule -^^^^^^^^^^^^^^^^^^ - -Provides information about the cost of different positions and moves. -""" import math from typing import TYPE_CHECKING, List, Optional, Tuple @@ -28,6 +22,8 @@ class CostmapCapsule: + """Provides information about the cost of different positions and moves.""" + def __init__(self, blackboard: "BodyBlackboard"): self._blackboard = blackboard self._node = blackboard.node @@ -137,11 +133,11 @@ def get_pass_regions(self) -> np.ndarray: def field_2_costmap_coord(self, x: float, y: float) -> Tuple[float, float]: """ - Converts a field position to the coresponding indices for the costmap. + Converts a field position to the corresponding indices for the costmap. :param x: X Position relative to the center point. (Positive is towards the enemy goal) :param y: Y Position relative to the center point. (Positive is towards the left when we face the enemy goal) - :return: The x index of the coresponding costmap slot, The y index of the coresponding costmap slot + :return: The x index of the corresponding costmap slot, The y index of the corresponding costmap slot """ idx_x = int( min( @@ -192,7 +188,7 @@ def cost_at_relative_xy(self, x: float, y: float) -> float: def calc_base_costmap(self): """ - Builds the base costmap based on the bahavior parameters. + 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 """ # Get parameters @@ -265,23 +261,23 @@ def calc_base_costmap(self): ] ) - # Apply map margin to fixpoints + # Apply map margin to fix points fix_points = [((p[0][0] + self.map_margin, p[0][1] + self.map_margin), p[1]) for p in fix_points] - # Interpolate the keypoints from above to form the costmap + # Interpolate the key points from above to form the costmap interpolated = griddata( [p[0] for p in fix_points], [p[1] for p in fix_points], (grid_x, grid_y), method="linear" ) - # Smooth the costmap to get more continus gradients + # Smooth the costmap to get more continuous gradients self.base_costmap = gaussian_filter(interpolated, self.body_config["base_costmap_smoothing_sigma"]) self.costmap = self.base_costmap.copy() def get_gradient_at_field_position(self, x: float, y: float) -> Tuple[float, float]: """ Gets the gradient tuple at a given field position - :param x: Field coordiante in the x direction - :param y: Field coordiante in the y direction + :param x: Field coordinate in the x direction + :param y: Field coordinate in the y direction """ 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] @@ -298,8 +294,8 @@ def get_cost_at_field_position(self, x: float, y: float) -> float: def get_gradient_direction_at_field_position(self, x: float, y: float): """ Returns the gradient direction at the given position - :param x: Field coordiante in the x direction - :param y: Field coordiante in the y direction + :param x: Field coordinate in the x direction + :param y: Field coordinate in the y direction """ # for debugging only # if False and self.costmap.sum() > 0: @@ -317,8 +313,8 @@ def get_gradient_direction_at_field_position(self, x: float, y: float): def get_cost_of_kick_relative(self, x: float, y: float, direction: float, kick_length: float, angular_range: float): """ Returns the cost of a kick at the given position and direction in base footprint frame - :param x: Field coordiante in the x direction - :param y: Field coordiante in the y direction + :param x: Field coordinate in the x direction + :param y: Field coordinate in the y direction :param direction: The direction of the kick :param kick_length: The length of the kick :param angular_range: The angular range of the kick""" @@ -346,8 +342,8 @@ def get_cost_of_kick_relative(self, x: float, y: float, direction: float, kick_l def get_cost_of_kick(self, x: float, y: float, direction: float, kick_length: float, angular_range: float) -> float: """ Returns the cost of the kick at the given position - :param x: Field coordiante in the x direction - :param y: Field coordiante in the y direction + :param x: Field coordinate in the x direction + :param y: Field coordinate in the y direction :param direction: The direction of the kick :param kick_length: The length of the kick :param angular_range: The angular range of the kick @@ -378,7 +374,7 @@ def get_cost_of_kick(self, x: float, y: float, direction: float, kick_length: fl # plt.show() # The main influence should be the maximum cost in the area which is covered by the kick. This could be the field boundary, robots, ... - # But we also want prio directions with lower min cost. This could be the goal area or the pass accept area of an teammate + # But we also want prioritize directions with lower min cost. This could be the goal area or the pass accept area of an teammate # 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 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 7a3eaa9a2..6e4b4a982 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,15 +1,11 @@ -""" -GameStatusCapsule -^^^^^^^^^^^^^^^^^ - -Provides information about the current game state. -""" from bitbots_utils.utils import get_parameters_from_other_node from game_controller_hl_interfaces.msg import GameState from rclpy.node import Node class GameStatusCapsule: + """Provides information about the current game state.""" + def __init__(self, node: Node): self.node = node self.team_id = get_parameters_from_other_node(self.node, "parameter_blackboard", ["team_id"])["team_id"] 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 f70bb22b0..c364b9e38 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py @@ -1,9 +1,3 @@ -""" -KickCapsule -^^^^^^^^^^^^^^^^ - -Communicates with the dynamic_kick. -""" from typing import TYPE_CHECKING, Optional if TYPE_CHECKING: @@ -18,6 +12,8 @@ class KickCapsule: + """Communicates with the dynamic_kick action server to kick the ball.""" + __blackboard: "BodyBlackboard" last_feedback: Optional[Kick.Feedback] = None 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 ab53ef473..4b80ba626 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py @@ -1,9 +1,3 @@ -""" -MiscCapsule -^^^^^^^^^^^ - -Capsule for miscellaneous things that don't fit anywhere else. -""" from typing import Optional from bitbots_utils.utils import get_parameters_from_other_node @@ -15,6 +9,8 @@ class MiscCapsule: + """Capsule for miscellaneous functions.""" + def __init__(self, node: Node): self.node = node self.head_pub = node.create_publisher(HeadMode, "head_mode", 10) 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 c45264794..8f39c0081 100644 --- a/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py +++ b/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py @@ -1,10 +1,3 @@ -""" -PathfindingCapsule -^^^^^^^^^^^ - -Capsule that handles everything related to pathfinding. -""" - import math from enum import Enum from typing import TYPE_CHECKING, Optional @@ -31,6 +24,8 @@ class BallGoalType(Enum): class PathfindingCapsule: + """Capsule for pathfinding related functions.""" + def __init__(self, blackboard: "BodyBlackboard", node: Node): self.node = node self._blackboard = blackboard 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 321407eaa..e857c2c61 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,7 +1,3 @@ -""" -TeamDataCapsule -^^^^^^^^^^^^^^^ -""" from typing import Dict, List, Optional, Tuple import numpy as np @@ -19,6 +15,8 @@ class TeamDataCapsule: def __init__(self, node: Node): + """Handles incoming team data communication.""" + self.node = node # Publishers 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 1ef33a1c3..576f978bd 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,10 +1,3 @@ -""" -WorldModelCapsule -^^^^^^^^^^^^^^^^^^ - -Provides information about the world model. -""" - import math from typing import TYPE_CHECKING, Dict, Optional, Tuple @@ -32,6 +25,8 @@ class WorldModelCapsule: + """Provides information about the world model.""" + def __init__(self, blackboard: "BodyBlackboard"): self._blackboard = blackboard self.body_config = get_parameter_dict(self._blackboard.node, "body") diff --git a/bitbots_behavior/bitbots_blackboard/package.xml b/bitbots_behavior/bitbots_blackboard/package.xml index b055003b3..169071b84 100644 --- a/bitbots_behavior/bitbots_blackboard/package.xml +++ b/bitbots_behavior/bitbots_blackboard/package.xml @@ -20,7 +20,6 @@ Hamburg Bit-Bots - bitbots_docs bitbots_tf_listener bitbots_utils @@ -44,6 +43,6 @@ unknown python3 - ament_cmake - + ament_cmake + diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py index 4a5767a2f..c7708c445 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py +++ b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py @@ -1,13 +1,3 @@ -#!/usr/bin/env python3 - -""" -BehaviorModule -^^^^^^^^^^^^^^ -.. moduleauthor:: Martin Poppinga <1popping@informatik.uni-hamburg.de> - -Starts the body behavior -""" - import os import rclpy diff --git a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index 537131b36..4a186362e 100644 --- a/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml +++ b/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml @@ -148,7 +148,7 @@ # Range in which the ball far approach point is counted as reached ball_far_approach_position_thresh: 0.2 - # We reapproach the ball after it has moved further away than this distance. This includes moveing to the far approach position. + # We reapproach the ball after it has moved further away than this distance. This includes moving to the far approach position. ball_reapproach_dist: 1.0 # Distance at which the ball is normally approached