From 46e7004da34829afae25cba8a4d00b04c70c4162 Mon Sep 17 00:00:00 2001 From: Timon Engelke Date: Fri, 7 Jul 2023 21:21:46 +0200 Subject: [PATCH 1/5] feat(pre-commit): add linting/formating checks by usage of the [pre-commit](https://pre-commit.com/) framework. We are including the following: - `ruff` for fast python linting - `black` for opinionated python auto formatting - `clang-format` for C++ formatting based on google styles - `cppcheck` for C++ linting --- .clang-format | 4 ++++ .pre-commit-config.yaml | 22 ++++++++++++++++++++++ pyproject.toml | 14 ++++++++++++++ 3 files changed, 40 insertions(+) create mode 100644 .clang-format create mode 100644 .pre-commit-config.yaml create mode 100644 pyproject.toml diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..3442bc76 --- /dev/null +++ b/.clang-format @@ -0,0 +1,4 @@ +--- +Language: Cpp +BasedOnStyle: Google +ColumnLimit: 120 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..f5e42d68 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,22 @@ +repos: +- repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.1.6 + hooks: + - id: ruff + args: + - "--fix" + - "--exit-non-zero-on-fix" +- repo: https://github.com/psf/black + rev: 23.11.0 + hooks: + - id: black +- repo: https://github.com/pocc/pre-commit-hooks + rev: v1.3.5 + hooks: + - id: clang-format + args: + - "-i" + - id: cppcheck + args: + - "--suppress=missingInclude" + - "--suppress=missingIncludeSystem" diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 00000000..3ed6ff7a --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,14 @@ +[tool.black] +line-length = 120 + +[tool.ruff] +line-length = 120 + # Never enforce `E501` (line length violations), as black takes care of this. +ignore = ["E501"] +# Additionally enable the following rules +# - pycodestyle warnings (`W`) +# - flake8-bugbear warnings (`B`) +# - isort import sorting (`I`) +# - pep8-naming convenrtions (`N`) +# - pyupgrade prefer newer language constructs (`UP`) +select = ["F", "E", "B", "W", "I", "N", "UP"] From 7a5244e813ca2299c8ef9a22e14053f86b621718 Mon Sep 17 00:00:00 2001 From: Timon Engelke Date: Fri, 7 Jul 2023 23:35:38 +0200 Subject: [PATCH 2/5] ci(pre-commit): add github action for linting/formatting that will run and check the pre-commit hooks on PRs and updates on the master branch --- .github/workflows/pre-commit.yml | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 .github/workflows/pre-commit.yml diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 00000000..a7bea7fa --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,16 @@ +name: Code style checks with pre-commit + +on: + pull_request: + push: + branches: [main] + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v4 + - name: Install cppcheck + run: sudo apt install cppcheck -y + - uses: pre-commit/action@v3.0.0 From 31956156a6cab20c1f54f976dac745d95a1254c0 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Thu, 23 Nov 2023 00:43:30 +0100 Subject: [PATCH 3/5] style(pre-commit): format all code with ruff/black --- .../bitbots_blackboard/blackboard.py | 11 +- .../capsules/animation_capsule.py | 22 +- .../capsules/costmap_capsule.py | 203 ++++++++++-------- .../capsules/game_status_capsule.py | 38 ++-- .../capsules/kick_capsule.py | 30 +-- .../capsules/misc_capsule.py | 11 +- .../capsules/pathfinding_capsule.py | 44 ++-- .../capsules/team_data_capsule.py | 100 +++++---- .../capsules/world_model_capsule.py | 136 +++++++----- bitbots_blackboard/docs/conf.py | 74 +++---- bitbots_blackboard/setup.py | 7 +- .../actions/avoid_ball.py | 18 +- .../actions/change_action.py | 23 +- .../actions/change_role.py | 8 +- .../actions/deactivate_hcm.py | 5 +- .../actions/dribble_forward.py | 31 ++- .../actions/forget_ball.py | 5 +- .../actions/get_walkready.py | 20 +- .../bitbots_body_behavior/actions/go_to.py | 21 +- .../actions/go_to_ball.py | 22 +- .../actions/go_to_block_position.py | 15 +- .../actions/go_to_corner_kick_position.py | 7 +- .../actions/go_to_defense_position.py | 20 +- .../actions/go_to_pass_position.py | 7 +- .../actions/go_to_role_position.py | 29 +-- .../actions/head_modes.py | 26 ++- .../actions/kick_ball.py | 61 +++--- .../actions/play_animation.py | 9 +- .../bitbots_body_behavior/actions/stand.py | 16 +- .../bitbots_body_behavior/actions/timer.py | 26 +-- .../bitbots_body_behavior/actions/turn.py | 16 +- .../bitbots_body_behavior/actions/walk.py | 5 +- .../bitbots_body_behavior/body_behavior.py | 10 +- .../decisions/aligned_to_goal.py | 22 +- .../decisions/avoid_ball.py | 12 +- .../decisions/ball_close.py | 20 +- .../decisions/ball_dangerous.py | 24 +-- .../decisions/ball_in_defensive_area.py | 31 +-- .../decisions/ball_kick_area.py | 30 +-- .../decisions/ball_seen.py | 19 +- .../decisions/closest_to_ball.py | 24 ++- .../decisions/config_role.py | 10 +- .../decisions/current_score.py | 6 +- .../decisions/do_once.py | 7 +- .../decisions/dribble_or_kick.py | 36 ++-- .../decisions/game_state_decider.py | 12 +- .../decisions/goal_scored.py | 20 +- .../decisions/goalie_handling_ball.py | 6 +- .../decisions/is_penalized.py | 9 +- .../decisions/kick_off_time_up.py | 22 +- .../decisions/last_player.py | 6 +- .../decisions/pass_started.py | 6 +- .../decisions/reached_goal.py | 12 +- .../decisions/secondary_state_decider.py | 33 +-- .../bitbots_body_behavior/decisions/timer.py | 37 ++-- bitbots_body_behavior/docs/conf.py | 74 +++---- bitbots_body_behavior/setup.py | 40 ++-- bitbots_body_behavior/test/test_dsd_file.py | 4 +- 58 files changed, 853 insertions(+), 745 deletions(-) diff --git a/bitbots_blackboard/bitbots_blackboard/blackboard.py b/bitbots_blackboard/bitbots_blackboard/blackboard.py index 2c05446d..46bf3bdb 100644 --- a/bitbots_blackboard/bitbots_blackboard/blackboard.py +++ b/bitbots_blackboard/bitbots_blackboard/blackboard.py @@ -1,14 +1,15 @@ +import tf2_ros as tf2 +from bitbots_utils.utils import get_parameter_dict +from rclpy.node import Node + from bitbots_blackboard.capsules.animation_capsule import AnimationCapsule -from bitbots_blackboard.capsules.misc_capsule import MiscCapsule +from bitbots_blackboard.capsules.costmap_capsule import CostmapCapsule from bitbots_blackboard.capsules.game_status_capsule import GameStatusCapsule from bitbots_blackboard.capsules.kick_capsule import KickCapsule +from bitbots_blackboard.capsules.misc_capsule import MiscCapsule from bitbots_blackboard.capsules.pathfinding_capsule import PathfindingCapsule from bitbots_blackboard.capsules.team_data_capsule import TeamDataCapsule from bitbots_blackboard.capsules.world_model_capsule import WorldModelCapsule -from bitbots_blackboard.capsules.costmap_capsule import CostmapCapsule -from bitbots_utils.utils import get_parameter_dict -from rclpy.node import Node -import tf2_ros as tf2 class BodyBlackboard: diff --git a/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py b/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py index 2d760bff..b945eae5 100644 --- a/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py +++ b/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py @@ -4,13 +4,12 @@ Communicates with the animation action server and plays predefined animations. """ +from bitbots_msgs.action import Dynup, LookAt, PlayAnimation from rclpy.action import ActionClient from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.duration import Duration from rclpy.node import Node -from bitbots_msgs.action import Dynup, LookAt, PlayAnimation - class AnimationCapsule: def __init__(self, node: Node): @@ -25,17 +24,9 @@ def __init__(self, node: Node): self.cheering_animation: str = self.node.get_parameter("Animations.Misc.cheering").value self.init_animation: str = self.node.get_parameter("Animations.Misc.init").value - self.animation_client = ActionClient( - node, - PlayAnimation, - 'animation', - callback_group=ReentrantCallbackGroup()) + self.animation_client = ActionClient(node, PlayAnimation, "animation", callback_group=ReentrantCallbackGroup()) - self.dynup_action_client = ActionClient( - node, - Dynup, - "dynup", - callback_group=ReentrantCallbackGroup()) + self.dynup_action_client = ActionClient(node, Dynup, "dynup", callback_group=ReentrantCallbackGroup()) self.lookat_action_client = ActionClient(node, LookAt, "look_at_goal") @@ -56,15 +47,16 @@ def play_animation(self, animation: str, from_hcm: bool) -> bool: if not self.animation_client.wait_for_server(Duration(seconds=10)): self.node.get_logger().error( - "Animation Action Server not running! Motion can not work without animation action server.") + "Animation Action Server not running! Motion can not work without animation action server." + ) return False goal = PlayAnimation.Goal() goal.animation = animation goal.hcm = from_hcm # the animation is from the hcm self.animation_client.send_goal_async(goal).add_done_callback( - lambda future: future.result().get_result_async().add_done_callback( - lambda _: self.__done_cb())) + lambda future: future.result().get_result_async().add_done_callback(lambda _: self.__done_cb()) + ) self.active = True diff --git a/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py b/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py index dca8c45f..9d807541 100644 --- a/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py +++ b/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py @@ -10,23 +10,21 @@ if TYPE_CHECKING: from bitbots_blackboard.blackboard import BodyBlackboard -import matplotlib.pyplot as plt import numpy as np import tf2_ros as tf2 -from bitbots_utils.utils import (get_parameter_dict, - get_parameters_from_other_node) +from bitbots_utils.utils import get_parameter_dict, get_parameters_from_other_node from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion from nav_msgs.msg import MapMetaData, OccupancyGrid from PIL import Image, ImageDraw from rclpy.clock import ClockType from rclpy.duration import Duration from rclpy.time import Time -from ros2_numpy import numpify, msgify +from ros2_numpy import msgify, numpify from scipy.interpolate import griddata from scipy.ndimage import gaussian_filter from soccer_vision_3d_msgs.msg import Robot, RobotArray -from tf_transformations import euler_from_quaternion, quaternion_from_euler from tf2_geometry_msgs import PointStamped +from tf_transformations import euler_from_quaternion, quaternion_from_euler class CostmapCapsule: @@ -35,23 +33,21 @@ def __init__(self, blackboard: "BodyBlackboard"): self._node = blackboard.node self.body_config = get_parameter_dict(self._blackboard.node, "body") - self.map_frame: str = self._blackboard.node.get_parameter('map_frame').value - self.base_footprint_frame: str = self._blackboard.node.get_parameter('base_footprint_frame').value + self.map_frame: str = self._blackboard.node.get_parameter("map_frame").value + self.base_footprint_frame: str = self._blackboard.node.get_parameter("base_footprint_frame").value parameters = get_parameters_from_other_node( - self._blackboard.node, - "/parameter_blackboard", - ["field_length", "field_width", "goal_width"]) + self._blackboard.node, "/parameter_blackboard", ["field_length", "field_width", "goal_width"] + ) self.field_length: float = parameters["field_length"] self.field_width: float = parameters["field_width"] self.goal_width: float = parameters["goal_width"] - self.map_margin: float = self.body_config['map_margin'] - self.obstacle_costmap_smoothing_sigma: float = self.body_config['obstacle_costmap_smoothing_sigma'] - self.obstacle_cost: float = self.body_config['obstacle_cost'] - + self.map_margin: float = self.body_config["map_margin"] + self.obstacle_costmap_smoothing_sigma: float = self.body_config["obstacle_costmap_smoothing_sigma"] + self.obstacle_cost: float = self.body_config["obstacle_cost"] # Publisher for visualization in RViZ - self.costmap_publisher = self._blackboard.node.create_publisher(OccupancyGrid, 'debug/costmap', 1) + self.costmap_publisher = self._blackboard.node.create_publisher(OccupancyGrid, "debug/costmap", 1) self.base_costmap: Optional[np.ndarray] = None # generated once in constructor field features self.costmap: Optional[np.ndarray] = None # updated on the fly based on the base_costmap @@ -61,7 +57,6 @@ def __init__(self, blackboard: "BodyBlackboard"): self.calc_base_costmap() self.calc_gradients() - def robot_callback(self, msg: RobotArray): """ Callback with new robot detections @@ -75,8 +70,7 @@ def robot_callback(self, msg: RobotArray): idx_x, idx_y = self.field_2_costmap_coord(robot.bb.center.position.x, robot.bb.center.position.y) # TODO inflate # Draw obstacle with smoothing independent weight on obstacle costmap - obstacle_map[idx_x, idx_y] = \ - self.obstacle_cost * self.obstacle_costmap_smoothing_sigma + obstacle_map[idx_x, idx_y] = self.obstacle_cost * self.obstacle_costmap_smoothing_sigma # Smooth obstacle map obstacle_map = gaussian_filter(obstacle_map, self.obstacle_costmap_smoothing_sigma) # Get pass offsets @@ -92,8 +86,11 @@ def publish_costmap(self): Publishes the costmap for rviz """ # 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).astype(np.int8).T + normalized_costmap = ( + (255 - ((self.costmap - np.min(self.costmap)) / (np.max(self.costmap) - np.min(self.costmap))) * 255 / 2.1) + .astype(np.int8) + .T + ) # Build the OccupancyGrid message msg: OccupancyGrid = msgify( OccupancyGrid, @@ -105,7 +102,9 @@ def publish_costmap(self): x=-self.field_length / 2 - self.map_margin, y=-self.field_width / 2 - self.map_margin, ) - ))) + ), + ), + ) # Change the frame to allow namespaces msg.header.frame_id = self.map_frame # Publish @@ -144,10 +143,18 @@ def field_2_costmap_coord(self, x: float, y: float) -> Tuple[float, float]: :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 """ - idx_x = int(min(((self.field_length + self.map_margin * 2) * 10) - 1, - max(0, (x + self.field_length / 2 + self.map_margin) * 10))) - idx_y = int(min(((self.field_width + self.map_margin * 2) * 10) - 1, - max(0, (y + self.field_width / 2 + self.map_margin) * 10))) + idx_x = int( + min( + ((self.field_length + self.map_margin * 2) * 10) - 1, + max(0, (x + self.field_length / 2 + self.map_margin) * 10), + ) + ) + idx_y = int( + min( + ((self.field_width + self.map_margin * 2) * 10) - 1, + max(0, (y + self.field_width / 2 + self.map_margin) * 10), + ) + ) return idx_x, idx_y def calc_gradients(self): @@ -189,76 +196,85 @@ def calc_base_costmap(self): This costmap includes a gradient towards the enemy goal and high costs outside the playable area """ # Get parameters - goalpost_safety_distance: float = self.body_config["goalpost_safety_distance"] # offset in y direction from the goalpost - keep_out_border: float = self.body_config["keep_out_border"] # dangerous border area - in_field_value_our_side: float = self.body_config["in_field_value_our_side"] # start value on our side - corner_value: float = self.body_config["corner_value"] # cost in a corner - goalpost_value: float = self.body_config["goalpost_value"] # cost at a goalpost - goal_value: float = self.body_config["goal_value"] # cost in the goal + goalpost_safety_distance: float = self.body_config[ + "goalpost_safety_distance" + ] # offset in y direction from the goalpost + keep_out_border: float = self.body_config["keep_out_border"] # dangerous border area + in_field_value_our_side: float = self.body_config["in_field_value_our_side"] # start value on our side + corner_value: float = self.body_config["corner_value"] # cost in a corner + goalpost_value: float = self.body_config["goalpost_value"] # cost at a goalpost + goal_value: float = self.body_config["goal_value"] # cost in the goal # 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, + 0 : self.field_width + self.map_margin * 2 : (self.field_width + self.map_margin * 2) * 10j, + ] fix_points: List[Tuple[Tuple[float, float], float]] = [] # Add base points - fix_points.extend([ - # Corner points of the map (including margin) - ((-self.map_margin, -self.map_margin), - corner_value + in_field_value_our_side), - ((self.field_length + self.map_margin, -self.map_margin), - corner_value + in_field_value_our_side), - ((-self.map_margin, self.field_width + self.map_margin), - corner_value + in_field_value_our_side), - ((self.field_length + self.map_margin, self.field_width + self.map_margin), - corner_value + in_field_value_our_side), - # Corner points of the field - ((0, 0), - corner_value + in_field_value_our_side), - ((self.field_length, 0), - corner_value), - ((0, self.field_width), - corner_value + in_field_value_our_side), - ((self.field_length, self.field_width), - corner_value), - # Points in the field that pull the gradient down, so we don't play always in the middle - ((keep_out_border, keep_out_border), - in_field_value_our_side), - ((keep_out_border, self.field_width - keep_out_border), - in_field_value_our_side), - ]) + fix_points.extend( + [ + # Corner points of the map (including margin) + ((-self.map_margin, -self.map_margin), corner_value + in_field_value_our_side), + ((self.field_length + self.map_margin, -self.map_margin), corner_value + in_field_value_our_side), + ((-self.map_margin, self.field_width + self.map_margin), corner_value + in_field_value_our_side), + ( + (self.field_length + self.map_margin, self.field_width + self.map_margin), + corner_value + in_field_value_our_side, + ), + # Corner points of the field + ((0, 0), corner_value + in_field_value_our_side), + ((self.field_length, 0), corner_value), + ((0, self.field_width), corner_value + in_field_value_our_side), + ((self.field_length, self.field_width), corner_value), + # Points in the field that pull the gradient down, so we don't play always in the middle + ((keep_out_border, keep_out_border), in_field_value_our_side), + ((keep_out_border, self.field_width - keep_out_border), in_field_value_our_side), + ] + ) # Add goal area (including the dangerous parts on the side of the goal) - fix_points.extend([ - ((self.field_length, self.field_width / 2 - self.goal_width / 2), - goalpost_value), - ((self.field_length, self.field_width / 2 + self.goal_width / 2), - goalpost_value), - ((self.field_length, self.field_width / 2 - self.goal_width / 2 + goalpost_safety_distance), - goal_value), - ((self.field_length, self.field_width / 2 + self.goal_width / 2 - goalpost_safety_distance), - goal_value), - ((self.field_length + self.map_margin, - self.field_width / 2 - self.goal_width / 2 - goalpost_safety_distance), - -0.2), - ((self.field_length + self.map_margin, - self.field_width / 2 + self.goal_width / 2 + goalpost_safety_distance), - -0.2), - ]) + fix_points.extend( + [ + ((self.field_length, self.field_width / 2 - self.goal_width / 2), goalpost_value), + ((self.field_length, self.field_width / 2 + self.goal_width / 2), goalpost_value), + ( + (self.field_length, self.field_width / 2 - self.goal_width / 2 + goalpost_safety_distance), + goal_value, + ), + ( + (self.field_length, self.field_width / 2 + self.goal_width / 2 - goalpost_safety_distance), + goal_value, + ), + ( + ( + self.field_length + self.map_margin, + self.field_width / 2 - self.goal_width / 2 - goalpost_safety_distance, + ), + -0.2, + ), + ( + ( + self.field_length + self.map_margin, + self.field_width / 2 + self.goal_width / 2 + goalpost_safety_distance, + ), + -0.2, + ), + ] + ) # Apply map margin to fixpoints 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 - interpolated = griddata([p[0] for p in fix_points], [p[1] for p in fix_points], (grid_x, grid_y), - method='linear') + 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 - self.base_costmap = gaussian_filter( - interpolated, - self.body_config["base_costmap_smoothing_sigma"]) + 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]: @@ -286,7 +302,7 @@ def get_gradient_direction_at_field_position(self, x: float, y: float): :param y: Field coordiante in the y direction """ # for debugging only - #if False and self.costmap.sum() > 0: + # if False and self.costmap.sum() > 0: # # Create Grid # grid_x, grid_y = np.mgrid[0:self.field_length:self.field_length * 10j, # 0:self.field_width:self.field_width * 10j] @@ -338,7 +354,7 @@ def get_cost_of_kick(self, x: float, y: float, direction: float, kick_length: fl """ # 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])) + mask = Image.new("L", (self.costmap.shape[1], self.costmap.shape[0])) # draw kick area on mask with ones maskd = ImageDraw.Draw(mask) @@ -375,7 +391,9 @@ def get_current_cost_of_kick(self, direction: float, kick_length: float, angular """ return self.get_cost_of_kick_relative(0, 0, direction, kick_length, angular_range) - def get_best_kick_direction(self, min_angle: float, max_angle: float, num_kick_angles: int, kick_length: float, angular_range: float) -> float: + def get_best_kick_direction( + self, min_angle: float, max_angle: float, num_kick_angles: int, kick_length: float, angular_range: float + ) -> float: """ Returns the best kick direction in the given range :param min_angle: The minimum angle of the kick @@ -386,16 +404,17 @@ def get_best_kick_direction(self, min_angle: float, max_angle: float, num_kick_a """ # list of possible kick directions, sorted by absolute value to # prefer forward kicks to side kicks if their costs are equal - kick_directions = sorted(np.linspace(min_angle, - max_angle, - num=num_kick_angles), key=abs) + kick_directions = sorted(np.linspace(min_angle, max_angle, num=num_kick_angles), key=abs) # get the kick direction with the least cost - kick_direction = kick_directions[np.argmin([self.get_current_cost_of_kick(direction=direction, - kick_length=kick_length, - angular_range=angular_range) - for direction in kick_directions])] + kick_direction = kick_directions[ + np.argmin( + [ + self.get_current_cost_of_kick( + direction=direction, kick_length=kick_length, angular_range=angular_range + ) + for direction in kick_directions + ] + ) + ] return kick_direction - - - diff --git a/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py b/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py index 49b37b23..28e7c209 100644 --- a/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py +++ b/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py @@ -4,16 +4,15 @@ Provides information about the current game state. """ -from rclpy.node import Node - from bitbots_msgs.msg import GameState from bitbots_utils.utils import get_parameters_from_other_node +from rclpy.node import Node class GameStatusCapsule: def __init__(self, node: Node): self.node = node - self.team_id = get_parameters_from_other_node(self.node, 'parameter_blackboard', ['team_id'])['team_id'] + self.team_id = get_parameters_from_other_node(self.node, "parameter_blackboard", ["team_id"])["team_id"] self.gamestate = GameState() self.last_update = 0 self.unpenalized_time = 0 @@ -22,8 +21,13 @@ def __init__(self, node: Node): 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] + 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): @@ -42,9 +46,10 @@ def has_kickoff(self): return self.gamestate.has_kick_off def has_penalty_kick(self): - 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 + 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): return self.gamestate.own_score @@ -60,12 +65,18 @@ def get_seconds_since_any_goal(self): def get_seconds_remaining(self): # 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) + return max( + self.gamestate.seconds_remaining - (self.node.get_clock().now().nanoseconds / 1e9 - self.last_update), 0 + ) def get_secondary_seconds_remaining(self): """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) + return max( + self.gamestate.secondary_seconds_remaining + - (self.node.get_clock().now().nanoseconds / 1e9 - self.last_update), + 0, + ) def get_seconds_since_last_drop_ball(self): """Returns the seconds since the last drop in""" @@ -101,8 +112,11 @@ def gamestate_callback(self, gs: GameState): if gs.rival_score > self.gamestate.rival_score: self.last_goal_time = self.node.get_clock().now().nanoseconds / 1e9 - if gs.secondary_state_mode == 2 and self.gamestate.secondary_state_mode != 2 \ - and gs.game_state == GameState.GAMESTATE_PLAYING: + if ( + gs.secondary_state_mode == 2 + and self.gamestate.secondary_state_mode != 2 + and gs.game_state == GameState.GAMESTATE_PLAYING + ): # secondary action is now executed but we will not see this in the new messages. # it will look like a normal kick off, but we need to remember that this is some sort of free kick # we set the kickoff value accordingly, then we will not be allowed to move if it is a kick for the others diff --git a/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py b/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py index 7b615003..bf082f39 100644 --- a/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py +++ b/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py @@ -9,15 +9,14 @@ if TYPE_CHECKING: from bitbots_blackboard.blackboard import BodyBlackboard +from bitbots_msgs.action import Kick from rclpy.action import ActionClient +from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.duration import Duration from rclpy.time import Time -from rclpy.callback_groups import ReentrantCallbackGroup - -from bitbots_msgs.action import Kick -class KickCapsule(): +class KickCapsule: __blackboard: "BodyBlackboard" last_feedback: Optional[Kick.Feedback] = None @@ -39,14 +38,18 @@ def __init__(self, blackboard): def connect(self): topic = self.__blackboard.config["dynamic_kick"]["topic"] - self.__blackboard.node.get_logger().info("Connecting {}.KickCapsule to bitbots_dynamic_kick ({})" - .format(str(self.__blackboard.__class__).split(".")[-1], topic)) - self.__action_client = ActionClient(self.__blackboard.node, Kick, topic, callback_group=ReentrantCallbackGroup()) - self.__connected = self.__action_client.wait_for_server( - self.__blackboard.config["dynamic_kick"]["wait_time"]) + self.__blackboard.node.get_logger().info( + "Connecting {}.KickCapsule to bitbots_dynamic_kick ({})".format( + str(self.__blackboard.__class__).split(".")[-1], topic + ) + ) + self.__action_client = ActionClient( + self.__blackboard.node, Kick, topic, callback_group=ReentrantCallbackGroup() + ) + self.__connected = self.__action_client.wait_for_server(self.__blackboard.config["dynamic_kick"]["wait_time"]) if not self.__connected: - self.__blackboard.node.get_logger().error("No dynamic_kick server running on {}".format(topic)) + self.__blackboard.node.get_logger().error(f"No dynamic_kick server running on {topic}") def kick(self, goal: Kick.Goal): """ @@ -57,14 +60,15 @@ def kick(self, goal: Kick.Goal): if not self.__connected: # try to connect again self.__connected = self.__action_client.wait_for_server( - Duration(seconds=self.__blackboard.config["dynamic_kick"]["wait_time"])) + Duration(seconds=self.__blackboard.config["dynamic_kick"]["wait_time"]) + ) if not self.__connected: raise RuntimeError("Not connected to any dynamic_kick server") self.is_currently_kicking = True self.__action_client.send_goal_async(goal, self.__feedback_cb).add_done_callback( - lambda future: future.result().get_result_async().add_done_callback( - lambda _: self.__done_cb())) + lambda future: future.result().get_result_async().add_done_callback(lambda _: self.__done_cb()) + ) self.last_goal = goal self.last_goal_sent = self.__blackboard.node.get_clock().now() diff --git a/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py b/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py index b027ad02..87aac134 100644 --- a/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py +++ b/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py @@ -6,12 +6,12 @@ """ from typing import Optional +from bitbots_msgs.msg import HeadMode, RobotControlState +from bitbots_utils.utils import get_parameters_from_other_node from rclpy.duration import Duration from rclpy.node import Node from std_msgs.msg import Bool -from bitbots_msgs.msg import HeadMode, RobotControlState -from bitbots_utils.utils import get_parameters_from_other_node class MiscCapsule: def __init__(self, node: Node): @@ -20,10 +20,11 @@ def __init__(self, node: Node): # Config gamestate_settings = get_parameters_from_other_node( - self.node, 'parameter_blackboard', ['bot_id', 'position_number']) + self.node, "parameter_blackboard", ["bot_id", "position_number"] + ) - self.position_number: int = gamestate_settings['position_number'] - self.bot_id: int = gamestate_settings['bot_id'] + self.position_number: int = gamestate_settings["position_number"] + self.bot_id: int = gamestate_settings["bot_id"] self.robot_control_state: Optional[RobotControlState] = None self.timers = dict() diff --git a/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py b/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py index fc691786..58bfce66 100644 --- a/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py +++ b/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py @@ -13,16 +13,15 @@ from bitbots_blackboard.blackboard import BodyBlackboard import numpy as np +from bitbots_utils.transforms import quat_from_yaw +from bitbots_utils.utils import get_parameters_from_other_node from geometry_msgs.msg import Point, PoseStamped, Twist from rclpy.node import Node from ros2_numpy import numpify -from std_msgs.msg import Empty, Bool +from std_msgs.msg import Bool, Empty from tf_transformations import euler_from_quaternion from visualization_msgs.msg import Marker -from bitbots_utils.transforms import quat_from_yaw -from bitbots_utils.utils import get_parameters_from_other_node - # Type of pathfinding goal relative to the ball class BallGoalType(Enum): @@ -35,9 +34,9 @@ class PathfindingCapsule: def __init__(self, blackboard: "BodyBlackboard", node: Node): self.node = node self._blackboard = blackboard - self.map_frame: str = self.node.get_parameter('map_frame').value - self.position_threshold: str = self.node.get_parameter('body.pathfinding_position_threshold').value - self.orientation_threshold: str = self.node.get_parameter('body.pathfinding_orientation_threshold').value + self.map_frame: str = self.node.get_parameter("map_frame").value + self.position_threshold: str = self.node.get_parameter("body.pathfinding_position_threshold").value + self.orientation_threshold: str = self.node.get_parameter("body.pathfinding_orientation_threshold").value self.direct_cmd_vel_pub = self.node.create_publisher(Twist, "cmd_vel", 1) self.pathfinding_pub = node.create_publisher(PoseStamped, "goal_pose", 1) self.pathfinding_cancel_pub = node.create_publisher(Empty, "pathfinding/cancel", 1) @@ -47,9 +46,8 @@ def __init__(self, blackboard: "BodyBlackboard", node: Node): self.avoid_ball: bool = True self.current_cmd_vel = Twist() self.orient_to_ball_distance: float = get_parameters_from_other_node( - self.node, - 'bitbots_path_planning', - ['controller.orient_to_goal_distance'])["controller.orient_to_goal_distance"] + self.node, "bitbots_path_planning", ["controller.orient_to_goal_distance"] + )["controller.orient_to_goal_distance"] def publish(self, msg: PoseStamped): """ @@ -99,9 +97,11 @@ def calculate_time_to_ball(self): """ # only send new request if previous request is finished or first update # also verify that the ball and the localization are reasonably recent/accurate - if self._blackboard.world_model.ball_has_been_seen() and \ - self._blackboard.world_model.localization_precision_in_threshold(): - ball_target = self.get_ball_goal(BallGoalType.MAP, self._blackboard.config['ball_approach_dist']) + if ( + self._blackboard.world_model.ball_has_been_seen() + and self._blackboard.world_model.localization_precision_in_threshold() + ): + ball_target = self.get_ball_goal(BallGoalType.MAP, self._blackboard.config["ball_approach_dist"]) own_position = self._blackboard.world_model.get_current_position_pose_stamped() self._blackboard.team_data.own_time_to_ball = self.time_from_pose_to_pose(own_position, ball_target) else: @@ -121,9 +121,10 @@ def time_from_pose_to_pose(self, own_pose: PoseStamped, goal_pose: PoseStamped) _, _, start_theta = self._blackboard.world_model.get_current_position() goal_theta = euler_from_quaternion(numpify(goal_pose.pose.orientation))[2] start_goal_theta_diff = (abs(start_theta - goal_theta) + math.tau / 2) % math.tau - math.tau / 2 - start_goal_theta_cost = start_goal_theta_diff * self._blackboard.config[ - 'time_to_ball_cost_start_to_goal_angle'] - total_cost = path_length * self._blackboard.config['time_to_ball_cost_per_meter'] + start_goal_theta_cost + start_goal_theta_cost = ( + start_goal_theta_diff * self._blackboard.config["time_to_ball_cost_start_to_goal_angle"] + ) + total_cost = path_length * self._blackboard.config["time_to_ball_cost_per_meter"] + start_goal_theta_cost else: # calculate how much we need to turn to start walking along the path _, _, start_theta = self._blackboard.world_model.get_current_position() @@ -132,10 +133,13 @@ def time_from_pose_to_pose(self, own_pose: PoseStamped, goal_pose: PoseStamped) # calculate how much we need to turn to turn at the end of the path goal_theta = euler_from_quaternion(numpify(goal_pose.pose.orientation))[2] goal_theta_diff = (abs(goal_theta - path_theta) + math.tau / 2) % math.tau - math.tau / 2 - start_theta_cost = start_theta_diff * self._blackboard.config['time_to_ball_cost_start_angle'] - goal_theta_cost = goal_theta_diff * self._blackboard.config['time_to_ball_cost_goal_angle'] - total_cost = path_length * self._blackboard.config['time_to_ball_cost_per_meter'] + \ - start_theta_cost + goal_theta_cost + start_theta_cost = start_theta_diff * self._blackboard.config["time_to_ball_cost_start_angle"] + goal_theta_cost = goal_theta_diff * self._blackboard.config["time_to_ball_cost_goal_angle"] + total_cost = ( + path_length * self._blackboard.config["time_to_ball_cost_per_meter"] + + start_theta_cost + + goal_theta_cost + ) return total_cost def get_ball_goal(self, target: BallGoalType, distance: float) -> PoseStamped: diff --git a/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py b/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py index bf966ab7..7b9dce5d 100644 --- a/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py +++ b/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py @@ -5,6 +5,8 @@ from typing import Dict, List, Optional, Tuple import numpy as np +from bitbots_msgs.msg import Strategy, TeamData +from bitbots_utils.utils import get_parameters_from_other_node from geometry_msgs.msg import PointStamped, Pose from rclpy.clock import ClockType from rclpy.duration import Duration @@ -13,12 +15,8 @@ from ros2_numpy import numpify from std_msgs.msg import Float32 -from bitbots_msgs.msg import Strategy, TeamData -from bitbots_utils.utils import get_parameters_from_other_node - class TeamDataCapsule: - def __init__(self, node: Node): self.node = node @@ -27,12 +25,12 @@ def __init__(self, node: Node): self.time_to_ball_publisher = node.create_publisher(Float32, "time_to_ball", 2) # Retrieve game settings from parameter blackboard - params = get_parameters_from_other_node(self.node, 'parameter_blackboard', ['role']) - self.role = params['role'] + params = get_parameters_from_other_node(self.node, "parameter_blackboard", ["role"]) + self.role = params["role"] # Data # indexed with one to match robot ids - self.team_data : Dict[TeamData] = {} + self.team_data: Dict[TeamData] = {} for i in range(1, 7): self.team_data[i] = TeamData() self.team_strategy = dict() @@ -41,14 +39,14 @@ def __init__(self, node: Node): # Mapping self.roles_mapping = { - 'striker': Strategy.ROLE_STRIKER, - 'offense': Strategy.ROLE_STRIKER, - 'supporter': Strategy.ROLE_SUPPORTER, - 'defender': Strategy.ROLE_DEFENDER, - 'defense': Strategy.ROLE_DEFENDER, - 'other': Strategy.ROLE_OTHER, - 'goalie': Strategy.ROLE_GOALIE, - 'idle': Strategy.ROLE_IDLING + "striker": Strategy.ROLE_STRIKER, + "offense": Strategy.ROLE_STRIKER, + "supporter": Strategy.ROLE_SUPPORTER, + "defender": Strategy.ROLE_DEFENDER, + "defense": Strategy.ROLE_DEFENDER, + "other": Strategy.ROLE_OTHER, + "goalie": Strategy.ROLE_GOALIE, + "idle": Strategy.ROLE_IDLING, } # Possible actions @@ -60,7 +58,7 @@ def __init__(self, node: Node): Strategy.ACTION_WAITING, Strategy.ACTION_SEARCHING, Strategy.ACTION_KICKING, - Strategy.ACTION_LOCALIZING + Strategy.ACTION_LOCALIZING, } # The strategy which is communicated to the other robots @@ -72,31 +70,38 @@ def __init__(self, node: Node): # 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.ball_lost_time: float = Duration(seconds=self.node.get_parameter('body.ball_lost_time').value) - + self.ball_max_covariance: float = self.node.get_parameter("ball_max_covariance").value + self.ball_lost_time: float = Duration(seconds=self.node.get_parameter("body.ball_lost_time").value) + self.localization_precision_threshold_x_sdev: float = self.node.get_parameter( - 'body.localization_precision_threshold.x_sdev').value + "body.localization_precision_threshold.x_sdev" + ).value self.localization_precision_threshold_y_sdev: float = self.node.get_parameter( - 'body.localization_precision_threshold.y_sdev').value + "body.localization_precision_threshold.y_sdev" + ).value self.localization_precision_threshold_theta_sdev: float = self.node.get_parameter( - 'body.localization_precision_threshold.theta_sdev').value + "body.localization_precision_threshold.theta_sdev" + ).value def is_valid(self, data: TeamData) -> bool: """ Checks if a team data message from a given robot is valid. Meaning is is not too old and the robot is not penalized. """ - return self.node.get_clock().now() - Time.from_msg(data.header.stamp) < Duration(seconds=self.data_timeout) \ - and data.state != TeamData.STATE_PENALIZED + return ( + self.node.get_clock().now() - Time.from_msg(data.header.stamp) < Duration(seconds=self.data_timeout) + and data.state != TeamData.STATE_PENALIZED + ) def is_goalie_handling_ball(self): - """ Returns true if the goalie is going to the ball.""" + """Returns true if the goalie is going to the ball.""" data: TeamData for data in self.team_data.values(): - if self.is_valid(data) \ - and data.strategy.role == Strategy.ROLE_GOALIE \ - and data.strategy.action in [Strategy.ACTION_GOING_TO_BALL, Strategy.ACTION_KICKING]: + if ( + self.is_valid(data) + and data.strategy.role == Strategy.ROLE_GOALIE + and data.strategy.action in [Strategy.ACTION_GOING_TO_BALL, Strategy.ACTION_KICKING] + ): return True return False @@ -123,16 +128,20 @@ def team_rank_to_ball(self, own_ball_distance: float, count_goalies: bool = True # data should not be outdated, from a robot in play, only goalie if desired, # x and y covariance values should be below threshold. orientation covariance of ball does not matter # covariance is a 6x6 matrix as array. 0 is x, 7 is y - if self.is_valid(data) and ( - data.strategy.role != Strategy.ROLE_GOALIE or count_goalies) \ - and data.ball_absolute.covariance[0] < self.ball_max_covariance \ - and data.ball_absolute.covariance[7] < self.ball_max_covariance: + if ( + self.is_valid(data) + and (data.strategy.role != Strategy.ROLE_GOALIE or count_goalies) + and data.ball_absolute.covariance[0] < self.ball_max_covariance + and data.ball_absolute.covariance[7] < self.ball_max_covariance + ): if use_time_to_ball: distances.append(data.time_to_position_at_ball) else: - distances.append(np.linalg.norm( - numpify(data.ball_absolute.pose.position) - \ - numpify(data.robot_position.pose.position))) + distances.append( + np.linalg.norm( + numpify(data.ball_absolute.pose.position) - numpify(data.robot_position.pose.position) + ) + ) for rank, distance in enumerate(sorted(distances)): if own_ball_distance < distance: return rank + 1 @@ -155,7 +164,7 @@ def set_role(self, role: str): :param role: String describing the role, possible values are: ['goalie', 'offense', 'defense'] """ - assert role in ['goalie', 'offense', 'defense'] + assert role in ["goalie", "offense", "defense"] self.role = role self.strategy.role = self.roles_mapping[role] @@ -173,7 +182,7 @@ def get_kickoff_strategy(self) -> Tuple[int, float]: return self.strategy.offensive_side, self.strategy_update def get_active_teammate_poses(self, count_goalies: bool = False) -> List[Pose]: - """ Returns the poses of all playing robots """ + """Returns the poses of all playing robots""" poses = [] data: TeamData for data in self.team_data.values(): @@ -230,15 +239,16 @@ def std_dev_from_covariance(covariance): stamp = teamdata.header.stamp if self.node.get_clock().now() - Time.from_msg(stamp) < self.ball_lost_time: if ball_x_std_dev < self.ball_max_covariance and ball_y_std_dev < self.ball_max_covariance: - if robot_x_std_dev < self.localization_precision_threshold_x_sdev and \ - robot_y_std_dev < self.localization_precision_threshold_y_sdev and \ - robot_theta_std_dev < self.localization_precision_threshold_theta_sdev: + if ( + robot_x_std_dev < self.localization_precision_threshold_x_sdev + and robot_y_std_dev < self.localization_precision_threshold_y_sdev + and robot_theta_std_dev < self.localization_precision_threshold_theta_sdev + ): robot_dist = np.linalg.norm( - numpify(teamdata.ball_absolute.pose.position) - \ - numpify(teamdata.robot_position.pose.position)) + numpify(teamdata.ball_absolute.pose.position) + - numpify(teamdata.robot_position.pose.position) + ) if robot_dist < best_robot_dist: - best_ball = PointStamped( - header=teamdata.header, - point=teamdata.ball_absolute.pose.position) + best_ball = PointStamped(header=teamdata.header, point=teamdata.ball_absolute.pose.position) best_robot_dist = robot_dist return best_ball diff --git a/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py b/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py index 278ab6c3..273d21d2 100644 --- a/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py +++ b/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py @@ -12,19 +12,22 @@ import numpy as np import tf2_ros as tf2 -from bitbots_utils.utils import (get_parameter_dict, - get_parameters_from_other_node) -from geometry_msgs.msg import (PoseStamped, PoseWithCovarianceStamped, - TransformStamped, TwistStamped, - TwistWithCovarianceStamped) +from bitbots_utils.utils import get_parameter_dict, get_parameters_from_other_node +from geometry_msgs.msg import ( + PoseStamped, + PoseWithCovarianceStamped, + TransformStamped, + TwistStamped, + TwistWithCovarianceStamped, +) from rclpy.clock import ClockType from rclpy.duration import Duration from rclpy.time import Time -from ros2_numpy import numpify, msgify +from ros2_numpy import msgify, numpify from std_msgs.msg import Header from std_srvs.srv import Trigger +from tf2_geometry_msgs import Point, PointStamped from tf_transformations import euler_from_quaternion -from tf2_geometry_msgs import PointStamped, Point class WorldModelCapsule: @@ -34,10 +37,10 @@ def __init__(self, blackboard: "BodyBlackboard"): # This pose is not supposed to be used as robot pose. Just as precision measurement for the TF position. self.pose = PoseWithCovarianceStamped() - self.odom_frame: str = self._blackboard.node.get_parameter('odom_frame').value - self.map_frame: str = self._blackboard.node.get_parameter('map_frame').value - self.ball_frame: str = self._blackboard.node.get_parameter('ball_frame').value - self.base_footprint_frame: str = self._blackboard.node.get_parameter('base_footprint_frame').value + self.odom_frame: str = self._blackboard.node.get_parameter("odom_frame").value + self.map_frame: str = self._blackboard.node.get_parameter("map_frame").value + self.ball_frame: str = self._blackboard.node.get_parameter("ball_frame").value + self.base_footprint_frame: str = self._blackboard.node.get_parameter("base_footprint_frame").value self.ball = PointStamped() # The ball in the base footprint frame self.ball_odom = PointStamped() # The ball in the odom frame (when localization is not usable) @@ -49,12 +52,16 @@ def __init__(self, blackboard: "BodyBlackboard"): self.ball_teammate = PointStamped() self.ball_teammate.header.stamp = Time(clock_type=ClockType.ROS_TIME).to_msg() self.ball_teammate.header.frame_id = self.map_frame - self.ball_lost_time = Duration(seconds=self._blackboard.node.get_parameter('body.ball_lost_time').value) + self.ball_lost_time = Duration(seconds=self._blackboard.node.get_parameter("body.ball_lost_time").value) self.ball_twist_map: Optional[TwistStamped] = None self.ball_filtered: Optional[PoseWithCovarianceStamped] = None - self.ball_twist_lost_time = Duration(seconds=self._blackboard.node.get_parameter('body.ball_twist_lost_time').value) - self.ball_twist_precision_threshold = get_parameter_dict(self._blackboard.node, 'body.ball_twist_precision_threshold') - self.reset_ball_filter = self._blackboard.node.create_client(Trigger, 'ball_filter_reset') + self.ball_twist_lost_time = Duration( + seconds=self._blackboard.node.get_parameter("body.ball_twist_lost_time").value + ) + self.ball_twist_precision_threshold = get_parameter_dict( + self._blackboard.node, "body.ball_twist_precision_threshold" + ) + self.reset_ball_filter = self._blackboard.node.create_client(Trigger, "ball_filter_reset") self.counter: int = 0 self.ball_seen_time = Time(clock_type=ClockType.ROS_TIME) @@ -62,26 +69,26 @@ def __init__(self, blackboard: "BodyBlackboard"): self.ball_seen: bool = False self.ball_seen_teammate: bool = False parameters = get_parameters_from_other_node( - self._blackboard.node, - "/parameter_blackboard", - ["field_length", "field_width", "goal_width"]) + self._blackboard.node, "/parameter_blackboard", ["field_length", "field_width", "goal_width"] + ) self.field_length: float = parameters["field_length"] self.field_width: float = parameters["field_width"] self.goal_width: float = parameters["goal_width"] - self.map_margin: float = self._blackboard.node.get_parameter('body.map_margin').value + self.map_margin: float = self._blackboard.node.get_parameter("body.map_margin").value self.obstacle_costmap_smoothing_sigma: float = self._blackboard.node.get_parameter( - 'body.obstacle_costmap_smoothing_sigma').value - self.obstacle_cost: float = self._blackboard.node.get_parameter('body.obstacle_cost').value + "body.obstacle_costmap_smoothing_sigma" + ).value + self.obstacle_cost: float = self._blackboard.node.get_parameter("body.obstacle_cost").value self.localization_precision_threshold: Dict[str, float] = get_parameter_dict( - self._blackboard.node, 'body.localization_precision_threshold') + self._blackboard.node, "body.localization_precision_threshold" + ) # Publisher for visualization in RViZ - self.ball_publisher = self._blackboard.node.create_publisher(PointStamped, 'debug/viz_ball', 1) - self.ball_twist_publisher = self._blackboard.node.create_publisher(TwistStamped, 'debug/ball_twist', 1) - self.used_ball_pub = self._blackboard.node.create_publisher(PointStamped, 'debug/used_ball', 1) - self.which_ball_pub = self._blackboard.node.create_publisher(Header, 'debug/which_ball_is_used', 1) - + self.ball_publisher = self._blackboard.node.create_publisher(PointStamped, "debug/viz_ball", 1) + self.ball_twist_publisher = self._blackboard.node.create_publisher(TwistStamped, "debug/ball_twist", 1) + self.used_ball_pub = self._blackboard.node.create_publisher(PointStamped, "debug/used_ball", 1) + self.which_ball_pub = self._blackboard.node.create_publisher(Header, "debug/which_ball_is_used", 1) ############ ### Ball ### @@ -96,8 +103,9 @@ def ball_last_seen(self) -> Time: Returns the time at which the ball was last seen if it is in the threshold or the more recent ball from either the teammate or itself if teamcom is available """ - if not self.ball_seen_self() and \ - (hasattr(self._blackboard, "team_data") and self.localization_precision_in_threshold()): + if not self.ball_seen_self() and ( + hasattr(self._blackboard, "team_data") and self.localization_precision_in_threshold() + ): return max(self.ball_seen_time, self._blackboard.team_data.get_teammate_ball_seen_time()) else: return self.ball_seen_time @@ -112,7 +120,7 @@ def get_ball_position_xy(self) -> Tuple[float, float]: return ball.point.x, ball.point.y def get_ball_stamped_relative(self) -> PointStamped: - """ Returns the ball in the base_footprint frame i.e. relative to the robot projected on the ground""" + """Returns the ball in the base_footprint frame i.e. relative to the robot projected on the ground""" return self.ball def get_best_ball_point_stamped(self) -> PointStamped: @@ -130,10 +138,12 @@ def get_best_ball_point_stamped(self) -> PointStamped: return self.ball_map else: teammate_ball = self._blackboard.team_data.get_teammate_ball() - if teammate_ball is not None and self._blackboard.tf_buffer.can_transform(self.base_footprint_frame, - teammate_ball.header.frame_id, - teammate_ball.header.stamp, - timeout=Duration(seconds=0.2)): + if teammate_ball is not None and self._blackboard.tf_buffer.can_transform( + self.base_footprint_frame, + teammate_ball.header.frame_id, + teammate_ball.header.stamp, + timeout=Duration(seconds=0.2), + ): self.used_ball_pub.publish(teammate_ball) h = Header() h.stamp = teammate_ball.header.stamp @@ -142,7 +152,8 @@ def get_best_ball_point_stamped(self) -> PointStamped: return teammate_ball else: self._blackboard.node.get_logger().warning( - "our ball is bad but the teammates ball is worse or cant be transformed") + "our ball is bad but the teammates ball is worse or cant be transformed" + ) h = Header() h.stamp = self.ball_map.header.stamp h.frame_id = "own_ball_map" @@ -160,10 +171,12 @@ def get_best_ball_point_stamped(self) -> PointStamped: def get_ball_position_uv(self) -> Tuple[float, float]: ball = self.get_best_ball_point_stamped() try: - ball_bfp = self._blackboard.tf_buffer.transform(ball, self.base_footprint_frame, timeout=Duration(seconds=0.2)).point - except (tf2.ExtrapolationException) as e: + ball_bfp = self._blackboard.tf_buffer.transform( + ball, self.base_footprint_frame, timeout=Duration(seconds=0.2) + ).point + except tf2.ExtrapolationException as e: self._blackboard.node.get_logger().warn(e) - self._blackboard.node.get_logger().error('Severe transformation problem concerning the ball!') + self._blackboard.node.get_logger().error("Severe transformation problem concerning the ball!") return None return ball_bfp.x, ball_bfp.y @@ -190,16 +203,24 @@ def ball_filtered_callback(self, msg: PoseWithCovarianceStamped): # When the precision is not sufficient, the ball ages. x_sdev = msg.pose.covariance[0] # position 0,0 in a 6x6-matrix y_sdev = msg.pose.covariance[7] # position 1,1 in a 6x6-matrix - if x_sdev > self.body_config['ball_position_precision_threshold']['x_sdev'] or \ - y_sdev > self.body_config['ball_position_precision_threshold']['y_sdev']: + if ( + x_sdev > self.body_config["ball_position_precision_threshold"]["x_sdev"] + or y_sdev > self.body_config["ball_position_precision_threshold"]["y_sdev"] + ): self.forget_ball(own=True, team=False, reset_ball_filter=False) return ball_buffer = PointStamped(header=msg.header, point=msg.pose.pose.position) try: - self.ball = self._blackboard.tf_buffer.transform(ball_buffer, self.base_footprint_frame, timeout=Duration(seconds=1.0)) - self.ball_odom = self._blackboard.tf_buffer.transform(ball_buffer, self.odom_frame, timeout=Duration(seconds=1.0)) - self.ball_map = self._blackboard.tf_buffer.transform(ball_buffer, self.map_frame, timeout=Duration(seconds=1.0)) + self.ball = self._blackboard.tf_buffer.transform( + ball_buffer, self.base_footprint_frame, timeout=Duration(seconds=1.0) + ) + self.ball_odom = self._blackboard.tf_buffer.transform( + ball_buffer, self.odom_frame, timeout=Duration(seconds=1.0) + ) + self.ball_map = self._blackboard.tf_buffer.transform( + ball_buffer, self.map_frame, timeout=Duration(seconds=1.0) + ) # Set timestamps to zero to get the newest transform when this is transformed later self.ball_odom.header.stamp = Time(clock_type=ClockType.ROS_TIME).to_msg() self.ball_map.header.stamp = Time(clock_type=ClockType.ROS_TIME).to_msg() @@ -210,7 +231,7 @@ def ball_filtered_callback(self, msg: PoseWithCovarianceStamped): except (tf2.ConnectivityException, tf2.LookupException, tf2.ExtrapolationException) as e: self._blackboard.node.get_logger().warn(str(e)) - def recent_ball_twist_available(self) -> bool: + def recent_ball_twist_available(self) -> bool: if self.ball_twist_map is None: return False return self._blackboard.node.get_clock().now() - self.ball_twist_map.header.stamp < self.ball_twist_lost_time @@ -218,8 +239,10 @@ def recent_ball_twist_available(self) -> bool: def ball_twist_callback(self, msg: TwistWithCovarianceStamped): x_sdev = msg.twist.covariance[0] # position 0,0 in a 6x6-matrix y_sdev = msg.twist.covariance[7] # position 1,1 in a 6x6-matrix - if x_sdev > self.ball_twist_precision_threshold['x_sdev'] or \ - y_sdev > self.ball_twist_precision_threshold['y_sdev']: + if ( + x_sdev > self.ball_twist_precision_threshold["x_sdev"] + or y_sdev > self.ball_twist_precision_threshold["y_sdev"] + ): return if msg.header.frame_id != self.map_frame: try: @@ -353,8 +376,9 @@ def get_current_position_transform(self, frame_id: str) -> TransformStamped: :param frame_id: The frame_id to use for the position (e.g. 'map' or 'odom'), will default to map_frame """ try: - return self._blackboard.tf_buffer.lookup_transform(frame_id, self.base_footprint_frame, - Time(clock_type=ClockType.ROS_TIME)) + return self._blackboard.tf_buffer.lookup_transform( + frame_id, self.base_footprint_frame, Time(clock_type=ClockType.ROS_TIME) + ) except (tf2.LookupException, tf2.ConnectivityException, tf2.ExtrapolationException) as e: self._blackboard.node.get_logger().warn(str(e)) return None @@ -378,9 +402,11 @@ def localization_precision_in_threshold(self) -> bool: # get the standard deviation values of the covariance matrix precision = self.get_localization_precision() # return whether those values are in the threshold - return precision[0] < self.localization_precision_threshold['x_sdev'] and \ - precision[1] < self.localization_precision_threshold['y_sdev'] and \ - precision[2] < self.localization_precision_threshold['theta_sdev'] + return ( + precision[0] < self.localization_precision_threshold["x_sdev"] + and precision[1] < self.localization_precision_threshold["y_sdev"] + and precision[2] < self.localization_precision_threshold["theta_sdev"] + ) def localization_pose_current(self) -> bool: """ @@ -400,7 +426,7 @@ def localization_pose_current(self) -> bool: ########## def get_uv_from_xy(self, x, y) -> Tuple[float, float]: - """ Returns the relativ positions of the robot to this absolute position""" + """Returns the relativ positions of the robot to this absolute position""" current_position = self.get_current_position() x2 = x - current_position[0] y2 = y - current_position[1] @@ -410,16 +436,14 @@ def get_uv_from_xy(self, x, y) -> Tuple[float, float]: return u, v def get_xy_from_uv(self, u, v): - """ Returns the absolute position from the given relative position to the robot""" + """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): - """ Returns distance from robot to given position """ + """Returns distance from robot to given position""" u, v = self.get_uv_from_xy(x, y) dist = math.hypot(u, v) return dist - - diff --git a/bitbots_blackboard/docs/conf.py b/bitbots_blackboard/docs/conf.py index 586c7734..0bf6043d 100644 --- a/bitbots_blackboard/docs/conf.py +++ b/bitbots_blackboard/docs/conf.py @@ -1,4 +1,3 @@ -# -*- coding: utf-8 -*- # # Full list of options at http://www.sphinx-doc.org/en/master/config @@ -10,28 +9,30 @@ # import os import sys -import catkin_pkg.package +import catkin_pkg.package from exhale import utils package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) catkin_package = catkin_pkg.package.parse_package( - os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) + os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME) +) sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) # -- Helper functions -------------------------------------------------------- + def count_files(): """:returns tuple of (num_py, num_cpp)""" num_py = 0 num_cpp = 0 - for root, dirs, files in os.walk(os.path.join(package_dir, "src")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "src")): for f in files: if f.endswith(".py"): num_py += 1 - for root, dirs, files in os.walk(os.path.join(package_dir, "include")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "include")): for f in files: if f.endswith(".h") or f.endswith(".hpp"): num_cpp += 1 @@ -42,7 +43,7 @@ def count_files(): # -- Project information ----------------------------------------------------- project = catkin_package.name -copyright = '2019, Bit-Bots' +copyright = "2019, Bit-Bots" author = ", ".join([a.name for a in catkin_package.authors]) # The short X.Y version @@ -60,27 +61,27 @@ def count_files(): # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.doctest', - 'sphinx.ext.intersphinx', - 'sphinx.ext.todo', - 'sphinx.ext.coverage', - 'sphinx.ext.imgmath', - 'sphinx.ext.viewcode', - 'sphinx_rtd_theme', + "sphinx.ext.autodoc", + "sphinx.ext.doctest", + "sphinx.ext.intersphinx", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.imgmath", + "sphinx.ext.viewcode", + "sphinx_rtd_theme", ] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: # # source_suffix = ['.rst', '.md'] -source_suffix = '.rst' +source_suffix = ".rst" # The master toctree document. -master_doc = 'index' +master_doc = "index" # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -92,7 +93,7 @@ def count_files(): # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # The name of the Pygments (syntax highlighting) style to use. pygments_style = None @@ -108,24 +109,17 @@ def count_files(): if num_files_cpp > 0: extensions += [ - 'breathe', - 'exhale', + "breathe", + "exhale", ] - breathe_projects = { - project: os.path.join("_build", "doxyoutput", "xml") - } + breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} breathe_default_project = project def specifications_for_kind(kind): # Show all members for classes and structs if kind == "class" or kind == "struct": - return [ - ":members:", - ":protected-members:", - ":private-members:", - ":undoc-members:" - ] + return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] # An empty list signals to Exhale to use the defaults else: return [] @@ -136,13 +130,11 @@ def specifications_for_kind(kind): "rootFileName": "library_root.rst", "rootFileTitle": "C++ Library API", "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping( - specifications_for_kind - ), + "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), # Suggested optional arguments "createTreeView": True, "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")) + "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), } # -- Options for HTML output ------------------------------------------------- @@ -150,7 +142,7 @@ def specifications_for_kind(kind): # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'sphinx_rtd_theme' +html_theme = "sphinx_rtd_theme" # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -161,7 +153,7 @@ def specifications_for_kind(kind): # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] # Custom sidebar templates, must be a dictionary that maps document names # to template names. @@ -173,14 +165,14 @@ def specifications_for_kind(kind): # # html_sidebars = {} -html_logo = os.path.join('_static', 'logo.png') -html_favicon = os.path.join('_static', 'logo.png') +html_logo = os.path.join("_static", "logo.png") +html_favicon = os.path.join("_static", "logo.png") # -- Options for intersphinx extension --------------------------------------- # Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {'https://docs.python.org/': None} +intersphinx_mapping = {"https://docs.python.org/": None} # -- Options for todo extension ---------------------------------------------- @@ -188,6 +180,8 @@ def specifications_for_kind(kind): todo_include_todos = True # -- RST Standard variables --------------------------------------------------- -rst_prolog = ".. |project| replace:: {}\n".format(project) +rst_prolog = f".. |project| replace:: {project}\n" rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available") +rst_prolog += ".. |modindex| replace:: {}\n".format( + ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" +) diff --git a/bitbots_blackboard/setup.py b/bitbots_blackboard/setup.py index 2e59a33c..557c9215 100644 --- a/bitbots_blackboard/setup.py +++ b/bitbots_blackboard/setup.py @@ -1,10 +1,7 @@ -# -*- coding:utf-8 -*- from distutils.core import setup + from catkin_pkg.python_setup import generate_distutils_setup -d = generate_distutils_setup( - packages=['bitbots_blackboard'], - package_dir={'': 'src'} -) +d = generate_distutils_setup(packages=["bitbots_blackboard"], package_dir={"": "src"}) setup(**d) diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/avoid_ball.py b/bitbots_body_behavior/bitbots_body_behavior/actions/avoid_ball.py index 5ba73034..62cc9cb1 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/avoid_ball.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/avoid_ball.py @@ -1,15 +1,16 @@ +from dynamic_stack_decider.abstract_action_element import AbstractActionElement from std_msgs.msg import Bool -from bitbots_blackboard.blackboard import BodyBlackboard -from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard class AvoidBall(AbstractActionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters): - super(AvoidBall, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) self.blackboard = blackboard - self.active = parameters.get('active', True) + self.active = parameters.get("active", True) def perform(self, reevaluate=False): self.blackboard.pathfinding.ball_obstacle_active_pub.publish(Bool(data=self.active)) @@ -20,12 +21,11 @@ def perform(self, reevaluate=False): class AvoidBallActive(AvoidBall): def __init__(self, blackboard, dsd, parameters): - parameters['active'] = True - super(AvoidBallActive, self).__init__(blackboard, dsd, parameters) + parameters["active"] = True + super().__init__(blackboard, dsd, parameters) class AvoidBallInactive(AvoidBall): def __init__(self, blackboard, dsd, parameters): - parameters['active'] = False - super(AvoidBallInactive, self).__init__(blackboard, dsd, parameters) - + parameters["active"] = False + super().__init__(blackboard, dsd, parameters) diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/change_action.py b/bitbots_body_behavior/bitbots_body_behavior/actions/change_action.py index 0bce7c6a..ba0a2954 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/change_action.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/change_action.py @@ -1,31 +1,32 @@ -from dynamic_stack_decider import AbstractActionElement from bitbots_msgs.msg import Strategy +from dynamic_stack_decider import AbstractActionElement from bitbots_blackboard.blackboard import BodyBlackboard - class ChangeAction(AbstractActionElement): """ Changes the action communicated to the other robots. This action determines the behavior of the robot on a very high level and should not be confused with the DSD actions. """ + blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters): super().__init__(blackboard, dsd, parameters) self.blackboard = blackboard - self.action = parameters.get('action', None) + self.action = parameters.get("action", None) self.actions = { - 'undefined': Strategy.ACTION_UNDEFINED, - 'positioning': Strategy.ACTION_POSITIONING, - 'going_to_ball': Strategy.ACTION_GOING_TO_BALL, - 'kicking': Strategy.ACTION_KICKING, - 'searching': Strategy.ACTION_SEARCHING, - 'localizing': Strategy.ACTION_LOCALIZING, - 'trying_to_score': Strategy.ACTION_TRYING_TO_SCORE, - 'waiting': Strategy.ACTION_WAITING + "undefined": Strategy.ACTION_UNDEFINED, + "positioning": Strategy.ACTION_POSITIONING, + "going_to_ball": Strategy.ACTION_GOING_TO_BALL, + "kicking": Strategy.ACTION_KICKING, + "searching": Strategy.ACTION_SEARCHING, + "localizing": Strategy.ACTION_LOCALIZING, + "trying_to_score": Strategy.ACTION_TRYING_TO_SCORE, + "waiting": Strategy.ACTION_WAITING, } def perform(self, reevaluate=False): diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/change_role.py b/bitbots_body_behavior/bitbots_body_behavior/actions/change_role.py index 9a048ea5..c4552a2a 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/change_role.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/change_role.py @@ -1,18 +1,20 @@ -from bitbots_blackboard.blackboard import BodyBlackboard - from dynamic_stack_decider import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard + class ChangeRole(AbstractActionElement): """ Changes the role of the robot in the team. This can be e.g. 'goalie'. It is used to e.g. determine the role position. """ + blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters: dict): super().__init__(blackboard, dsd, parameters) - self.role = parameters.get('role', None) + self.role = parameters.get("role", None) def perform(self, reevaluate=False): self.blackboard.team_data.set_role(self.role) diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/deactivate_hcm.py b/bitbots_body_behavior/bitbots_body_behavior/actions/deactivate_hcm.py index ad22b38b..d51f9232 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/deactivate_hcm.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/deactivate_hcm.py @@ -1,11 +1,12 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider import AbstractActionElement from std_msgs.msg import Bool -from dynamic_stack_decider import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard class DeactivateHCM(AbstractActionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters): super().__init__(blackboard, dsd, parameters) diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/dribble_forward.py b/bitbots_body_behavior/bitbots_body_behavior/actions/dribble_forward.py index 0039dd6b..722d45b0 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/dribble_forward.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/dribble_forward.py @@ -1,21 +1,22 @@ import numpy as np -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_action_element import AbstractActionElement from geometry_msgs.msg import Twist -from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard class DribbleForward(AbstractActionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) - self.max_speed_x = self.blackboard.config['dribble_max_speed_x'] + self.max_speed_x = self.blackboard.config["dribble_max_speed_x"] self.min_speed_x = -0.1 - self.max_speed_y = self.blackboard.config['dribble_max_speed_y'] - self.ball_heading_x_vel_zero_point = self.blackboard.config['dribble_ball_heading_x_vel_zero_point'] - self.p = self.blackboard.config['dribble_p'] - self.max_accel_x = self.blackboard.config['dribble_accel_x'] - self.max_accel_y = self.blackboard.config['dribble_accel_y'] + self.max_speed_y = self.blackboard.config["dribble_max_speed_y"] + self.ball_heading_x_vel_zero_point = self.blackboard.config["dribble_ball_heading_x_vel_zero_point"] + self.p = self.blackboard.config["dribble_p"] + self.max_accel_x = self.blackboard.config["dribble_accel_x"] + self.max_accel_y = self.blackboard.config["dribble_accel_y"] self.current_speed_x = self.blackboard.pathfinding.current_cmd_vel.linear.x self.current_speed_y = self.blackboard.pathfinding.current_cmd_vel.linear.y @@ -34,19 +35,17 @@ def perform(self, reevaluate=False): # todo compute yaw speed based on how we are aligned to the goal - adaptive_acceleration_x = 1 - (abs(ball_angle) / self.ball_heading_x_vel_zero_point) - x_speed = self.max_speed_x * adaptive_acceleration_x + adaptive_acceleration_x = 1 - (abs(ball_angle) / self.ball_heading_x_vel_zero_point) + self.max_speed_x * adaptive_acceleration_x - self.current_speed_x = \ - self.max_accel_x * self.max_speed_x + \ - self.current_speed_x * (1 - self.max_accel_x) + self.current_speed_x = self.max_accel_x * self.max_speed_x + self.current_speed_x * (1 - self.max_accel_x) # give more speed in y direction based on ball position y_speed = ball_v * self.p - self.current_speed_y = \ - self.max_accel_y * np.clip(y_speed, -self.max_speed_y, self.max_speed_y) + \ - self.current_speed_y * (1 - self.max_accel_y) + self.current_speed_y = self.max_accel_y * np.clip( + y_speed, -self.max_speed_y, self.max_speed_y + ) + self.current_speed_y * (1 - self.max_accel_y) cmd_vel = Twist() cmd_vel.linear.x = self.current_speed_x diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/forget_ball.py b/bitbots_body_behavior/bitbots_body_behavior/actions/forget_ball.py index 3718024c..bcd071e8 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/forget_ball.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/forget_ball.py @@ -1,10 +1,11 @@ -from bitbots_blackboard.blackboard import BodyBlackboard - from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard + class ForgetBall(AbstractActionElement): blackboard: BodyBlackboard + def perform(self, reevaluate=False): self.blackboard.world_model.forget_ball(own=True, team=True, reset_ball_filter=True) self.pop() diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/get_walkready.py b/bitbots_body_behavior/bitbots_body_behavior/actions/get_walkready.py index 274f9c30..9f367b69 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/get_walkready.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/get_walkready.py @@ -1,15 +1,16 @@ import rclpy -from bitbots_blackboard.blackboard import BodyBlackboard - from bitbots_msgs.action import Dynup from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard + class GetWalkready(AbstractActionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) - self.direction = 'walkready' + self.direction = "walkready" self.first_perform = True self.active = False @@ -45,9 +46,10 @@ def start_animation(self) -> bool: if not server_running: while not server_running and rclpy.ok(): self.blackboard.node.get_logger().warn( - "Dynup Action Server not running! Dynup cannot work without dynup server! " - "Will now wait until server is accessible!", - throttle_duration_sec=10.0) + "Dynup Action Server not running! Dynup cannot work without dynup server! " + "Will now wait until server is accessible!", + throttle_duration_sec=10.0, + ) server_running = self.blackboard.animation.dynup_action_client.wait_for_server(timeout_sec=1) if server_running: self.blackboard.node.get_logger().warn("Dynup server now running, 'get_walkready' action will go on.") @@ -61,8 +63,10 @@ def start_animation(self) -> bool: self.active = True self.dynup_action_current_goal = self.blackboard.animation.dynup_action_client.send_goal_async(goal) self.dynup_action_current_goal.add_done_callback( - lambda future: future.result().get_result_async().add_done_callback( - lambda result_future: self.__done_cb(result_future))) + lambda future: future.result() + .get_result_async() + .add_done_callback(lambda result_future: self.__done_cb(result_future)) + ) return True def __done_cb(self, result_future): diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to.py b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to.py index e69088f4..b964a3a8 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to.py @@ -1,21 +1,20 @@ import math + import numpy as np import tf2_ros as tf2 -import tf2_geometry_msgs - -from rclpy.duration import Duration -from bitbots_blackboard.blackboard import BodyBlackboard -from geometry_msgs.msg import PoseStamped - from bitbots_utils.transforms import quat_from_yaw from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from geometry_msgs.msg import PoseStamped +from rclpy.duration import Duration + +from bitbots_blackboard.blackboard import BodyBlackboard class GoToRelativePosition(AbstractActionElement): blackboard: BodyBlackboard def __init__(self, blackboard, dsd, parameters: dict = None): - super(GoToRelativePosition, self).__init__(blackboard, dsd) + super().__init__(blackboard, dsd) 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)) self.first = True @@ -56,7 +55,7 @@ class GoToAbsolutePosition(AbstractActionElement): def __init__(self, blackboard, dsd, parameters=None): """Go to an absolute position on the field""" - super(GoToAbsolutePosition, self).__init__(blackboard, dsd) + super().__init__(blackboard, dsd) self.point = parameters def perform(self, reevaluate=False): @@ -75,7 +74,7 @@ def perform(self, reevaluate=False): class GoToOwnGoal(GoToAbsolutePosition): def __init__(self, blackboard, dsd, parameters=None): """Go to the own goal""" - super(GoToOwnGoal, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) self.point = ( self.blackboard.world_model.get_map_based_own_goal_center_xy()[0], self.blackboard.world_model.get_map_based_own_goal_center_xy()[1], @@ -86,7 +85,7 @@ def __init__(self, blackboard, dsd, parameters=None): class GoToEnemyGoal(GoToAbsolutePosition): def __init__(self, blackboard, dsd, parameters=None): """Go to the enemy goal""" - super(GoToEnemyGoal, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) self.point = ( self.blackboard.world_model.get_map_based_opp_goal_center_xy()[0], self.blackboard.world_model.get_map_based_opp_goal_center_xy()[1], @@ -97,5 +96,5 @@ def __init__(self, blackboard, dsd, parameters=None): class GoToCenterpoint(GoToAbsolutePosition): def __init__(self, blackboard, dsd, parameters=None): """Go to the center of the field and look towards the enemy goal""" - super(GoToCenterpoint, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) self.point = 0, 0, 0 diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_ball.py b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_ball.py index 2e6f9ac8..fe31431a 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_ball.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_ball.py @@ -1,28 +1,30 @@ -from bitbots_blackboard.blackboard import BodyBlackboard -from bitbots_blackboard.capsules.pathfinding_capsule import BallGoalType +from dynamic_stack_decider.abstract_action_element import AbstractActionElement from geometry_msgs.msg import Vector3 from rclpy.duration import Duration from std_msgs.msg import ColorRGBA from visualization_msgs.msg import Marker -from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard +from bitbots_blackboard.capsules.pathfinding_capsule import BallGoalType class GoToBall(AbstractActionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(GoToBall, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) - if 'target' not in parameters.keys(): - self.blackboard.node.get_logger().error('The parameter "target" could not be used to decide whether map information is accesible') + if "target" not in parameters.keys(): + self.blackboard.node.get_logger().error( + 'The parameter "target" could not be used to decide whether map information is accesible' + ) else: - self.target = BallGoalType(parameters['target']) + self.target = BallGoalType(parameters["target"]) - self.blocking = parameters.get('blocking', True) - self.distance = parameters.get('distance', self.blackboard.config['ball_approach_dist']) + self.blocking = parameters.get("blocking", True) + self.distance = parameters.get("distance", self.blackboard.config["ball_approach_dist"]) def perform(self, reevaluate=False): - pose_msg = self.blackboard.pathfinding.get_ball_goal(self.target, self.distance) self.blackboard.pathfinding.publish(pose_msg) diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py index f8f45a63..f2c623c9 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py @@ -1,15 +1,16 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_action_element import AbstractActionElement from tf2_geometry_msgs import PoseStamped -from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard class GoToBlockPosition(AbstractActionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(GoToBlockPosition, self).__init__(blackboard, dsd, parameters) - self.block_position_goal_offset = self.blackboard.config['block_position_goal_offset'] - self.block_position_gradient_factor = self.blackboard.config['block_position_gradient_factor'] + super().__init__(blackboard, dsd, parameters) + self.block_position_goal_offset = self.blackboard.config["block_position_goal_offset"] + self.block_position_gradient_factor = self.blackboard.config["block_position_gradient_factor"] def perform(self, reevaluate=False): # The block position should be a position between the ball and the center of the goal @@ -40,7 +41,9 @@ def perform(self, reevaluate=False): pose_msg.header.stamp = self.blackboard.node.get_clock().now().to_msg() pose_msg.header.frame_id = self.blackboard.map_frame - pose_msg.pose.position.x = float(-(self.blackboard.world_model.field_length / 2) + self.block_position_goal_offset) + pose_msg.pose.position.x = float( + -(self.blackboard.world_model.field_length / 2) + self.block_position_goal_offset + ) pose_msg.pose.position.y = float(self._stay_in_front_of_goal(goalie_y)) pose_msg.pose.orientation.w = 1.0 diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_corner_kick_position.py b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_corner_kick_position.py index 95d56578..6f7e27ae 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_corner_kick_position.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_corner_kick_position.py @@ -1,20 +1,21 @@ import math -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_action_element import AbstractActionElement from geometry_msgs.msg import Quaternion from tf2_geometry_msgs import PoseStamped from tf_transformations import quaternion_from_euler -from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard class GoToCornerKickPosition(AbstractActionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) # optional parameter which goes into the block position at a certain distance to the ball - self.mode = parameters.get('mode', None) + self.mode = parameters.get("mode", None) if self.mode is None or self.mode not in ("striker", "supporter", "others"): self.blackboard.node.get_logger().error("mode for corner kick not specified") exit() diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_defense_position.py b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_defense_position.py index 8348d84d..a6447af1 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_defense_position.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_defense_position.py @@ -1,30 +1,32 @@ import math import numpy as np -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_action_element import AbstractActionElement from geometry_msgs.msg import Quaternion from tf2_geometry_msgs import PoseStamped from tf_transformations import quaternion_from_euler -from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard class GoToDefensePosition(AbstractActionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(GoToDefensePosition, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) # Also apply offset from the ready positions to the defense positions - role_positions = self.blackboard.config['role_positions'] + role_positions = self.blackboard.config["role_positions"] try: - generalized_role_position = \ - role_positions[self.blackboard.team_data.role]["active"][str(self.blackboard.misc.position_number)] - except KeyError: - raise KeyError('Role position for {} not specified in config'.format(self.blackboard.team_data.role)) + generalized_role_position = role_positions[self.blackboard.team_data.role]["active"][ + str(self.blackboard.misc.position_number) + ] + except KeyError as e: + raise KeyError(f"Role position for {self.blackboard.team_data.role} not specified in config") from e self.y_offset = generalized_role_position[1] * self.blackboard.world_model.field_width / 2 # optional parameter which goes into the block position at a certain distance to the ball - self.mode = parameters.get('mode', None) + self.mode = parameters.get("mode", None) def perform(self, reevaluate=False): # The defense position should be a position between the ball and the own goal. diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_pass_position.py b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_pass_position.py index e10fe0f8..59906e5c 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_pass_position.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_pass_position.py @@ -1,12 +1,13 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from bitbots_utils.transforms import quat_from_yaw +from dynamic_stack_decider.abstract_action_element import AbstractActionElement from tf2_geometry_msgs import PoseStamped -from dynamic_stack_decider.abstract_action_element import AbstractActionElement -from bitbots_utils.transforms import quat_from_yaw +from bitbots_blackboard.blackboard import BodyBlackboard class AbstractGoToPassPosition(AbstractActionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, accept, parameters=None): super().__init__(blackboard, dsd, parameters) self.max_x = self.blackboard.config["supporter_max_x"] diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_role_position.py b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_role_position.py index d32ba213..323270c6 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_role_position.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_role_position.py @@ -1,30 +1,33 @@ -import random - -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_action_element import AbstractActionElement from tf2_geometry_msgs import PoseStamped -from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard class GoToRolePosition(AbstractActionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(GoToRolePosition, self).__init__(blackboard, dsd, parameters) - role_positions = self.blackboard.config['role_positions'] - kickoff_type = 'active' if self.blackboard.gamestate.has_kickoff() else 'passive' + super().__init__(blackboard, dsd, parameters) + role_positions = self.blackboard.config["role_positions"] + kickoff_type = "active" if self.blackboard.gamestate.has_kickoff() else "passive" try: - if self.blackboard.team_data.role == 'goalie': + if self.blackboard.team_data.role == "goalie": generalized_role_position = role_positions[self.blackboard.team_data.role] else: # players other than the goalie have multiple possible positions - generalized_role_position = role_positions[self.blackboard.team_data.role][kickoff_type][str(self.blackboard.misc.position_number)] - except KeyError: - raise KeyError('Role position for {} not specified in config'.format(self.blackboard.team_data.role)) + generalized_role_position = role_positions[self.blackboard.team_data.role][kickoff_type][ + str(self.blackboard.misc.position_number) + ] + except KeyError as e: + raise KeyError(f"Role position for {self.blackboard.team_data.role} not specified in config") from e # Adapt position to field size # TODO know where map frame is located - self.role_position = [generalized_role_position[0] * self.blackboard.world_model.field_length / 2, - generalized_role_position[1] * self.blackboard.world_model.field_width / 2] + self.role_position = [ + generalized_role_position[0] * self.blackboard.world_model.field_length / 2, + generalized_role_position[1] * self.blackboard.world_model.field_width / 2, + ] def perform(self, reevaluate=False): pose_msg = PoseStamped() diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/head_modes.py b/bitbots_body_behavior/bitbots_body_behavior/actions/head_modes.py index 49ed074e..a9887f00 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/head_modes.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/head_modes.py @@ -1,18 +1,20 @@ import rclpy -from bitbots_blackboard.blackboard import BodyBlackboard - -from dynamic_stack_decider.abstract_action_element import AbstractActionElement -from bitbots_msgs.msg import HeadMode from bitbots_msgs.action import LookAt +from bitbots_msgs.msg import HeadMode +from dynamic_stack_decider.abstract_action_element import AbstractActionElement + +from bitbots_blackboard.blackboard import BodyBlackboard class AbstractHeadModeElement(AbstractActionElement): """Abstract class used for type hinting""" + blackboard: BodyBlackboard class LookAtBall(AbstractHeadModeElement): """Search for Ball and track it if found""" + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) @@ -22,9 +24,10 @@ def perform(self): if not server_running: while not server_running and rclpy.ok(): self.blackboard.node.get_logger().warn( - "Lookat Action Server not running! Lookat cannot work without lookat server!" - "Will now wait until server is accessible!", - throttle_duration_sec=10.0) + "Lookat Action Server not running! Lookat cannot work without lookat server!" + "Will now wait until server is accessible!", + throttle_duration_sec=10.0, + ) server_running = self.blackboard.animation.lookat_action_client.wait_for_server(timeout_sec=1) if server_running: self.blackboard.node.get_logger().warn("Lookat server now running, 'look_at_ball' action will go on.") @@ -40,18 +43,23 @@ def perform(self): class SearchBall(AbstractHeadModeElement): """Look for ball""" + def perform(self): self.blackboard.misc.set_head_duty(HeadMode.BALL_MODE) return self.pop() + class LookAtFieldFeatures(AbstractHeadModeElement): """Look generally for all features on the field (ball, goals, corners, center point)""" + def perform(self): self.blackboard.misc.set_head_duty(HeadMode.FIELD_FEATURES) return self.pop() + class LookForward(AbstractHeadModeElement): """Simply look directly forward""" + def perform(self): self.blackboard.misc.set_head_duty(HeadMode.LOOK_FORWARD) return self.pop() @@ -59,6 +67,7 @@ def perform(self): class DontMoveHead(AbstractHeadModeElement): """Don't move the head""" + def perform(self): self.blackboard.misc.set_head_duty(HeadMode.DONT_MOVE) return self.pop() @@ -66,12 +75,15 @@ def perform(self): class LookAtBallPenalty(AbstractHeadModeElement): """Ball Mode adapted for Penalty Kick""" + def perform(self): self.blackboard.misc.set_head_duty(HeadMode.BALL_MODE_PENALTY) return self.pop() + class LookAtFront(AbstractHeadModeElement): """Search in front of the robot""" + def perform(self): self.blackboard.misc.set_head_duty(HeadMode.LOOK_FRONT) return self.pop() diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/kick_ball.py b/bitbots_body_behavior/bitbots_body_behavior/actions/kick_ball.py index b751801e..ab53d8e9 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/kick_ball.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/kick_ball.py @@ -1,29 +1,32 @@ -from bitbots_blackboard.blackboard import BodyBlackboard from bitbots_msgs.action import Kick from bitbots_utils.transforms import quat_from_yaw from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard + class AbstractKickAction(AbstractActionElement): blackboard: BodyBlackboard + def pop(self): self.blackboard.world_model.forget_ball(own=True, team=True, reset_ball_filter=True) - super(AbstractKickAction, self).pop() + super().pop() class KickBallStatic(AbstractKickAction): def __init__(self, blackboard, dsd, parameters=None): - super(KickBallStatic, self).__init__(blackboard, dsd, parameters) - if 'foot' not in parameters.keys(): + super().__init__(blackboard, dsd, parameters) + if "foot" not in parameters.keys(): # usually, we kick with the right foot - self.kick = 'kick_right' # TODO get actual name of parameter from some config - elif 'right' == parameters['foot']: - self.kick = 'kick_right' # TODO get actual name of parameter from some config - elif 'left' == parameters['foot']: - self.kick = 'kick_left' # TODO get actual name of parameter from some config + self.kick = "kick_right" # TODO get actual name of parameter from some config + elif "right" == parameters["foot"]: + self.kick = "kick_right" # TODO get actual name of parameter from some config + elif "left" == parameters["foot"]: + self.kick = "kick_left" # TODO get actual name of parameter from some config else: self.blackboard.node.get_logger().error( - 'The parameter \'{}\' could not be used to decide which foot should kick'.format(parameters['foot'])) + "The parameter '{}' could not be used to decide which foot should kick".format(parameters["foot"]) + ) def perform(self, reevaluate=False): if not self.blackboard.animation.is_busy(): @@ -36,20 +39,20 @@ class KickBallDynamic(AbstractKickAction): """ def __init__(self, blackboard, dsd, parameters=None): - super(KickBallDynamic, self).__init__(blackboard, dsd, parameters) - if parameters.get('type', None) == 'penalty': + super().__init__(blackboard, dsd, parameters) + if parameters.get("type", None) == "penalty": self.penalty_kick = True else: self.penalty_kick = False self._goal_sent = False - self.kick_length = self.blackboard.config['kick_cost_kick_length'] - self.angular_range = self.blackboard.config['kick_cost_angular_range'] - self.max_kick_angle = self.blackboard.config['max_kick_angle'] - self.num_kick_angles = self.blackboard.config['num_kick_angles'] - self.penalty_kick_angle = self.blackboard.config['penalty_kick_angle'] + self.kick_length = self.blackboard.config["kick_cost_kick_length"] + self.angular_range = self.blackboard.config["kick_cost_angular_range"] + self.max_kick_angle = self.blackboard.config["max_kick_angle"] + self.num_kick_angles = self.blackboard.config["num_kick_angles"] + self.penalty_kick_angle = self.blackboard.config["penalty_kick_angle"] # By default, don't reevaluate - self.never_reevaluate = parameters.get('r', True) and parameters.get('reevaluate', True) + self.never_reevaluate = parameters.get("r", True) and parameters.get("reevaluate", True) def perform(self, reevaluate=False): self.publish_debug_data("Currently Kicking", self.blackboard.kick.is_currently_kicking) @@ -59,7 +62,9 @@ def perform(self, reevaluate=False): goal.header.stamp = self.blackboard.node.get_clock().now().to_msg() # currently we use a tested left or right kick - goal.header.frame_id = self.blackboard.world_model.base_footprint_frame # the ball position is stated in this frame + goal.header.frame_id = ( + self.blackboard.world_model.base_footprint_frame + ) # the ball position is stated in this frame if self.penalty_kick: goal.kick_speed = 6.7 @@ -70,11 +75,8 @@ def perform(self, reevaluate=False): # only check 2 directions, left and right kick_direction = self.blackboard.costmap.get_best_kick_direction( - -self.penalty_kick_angle, - self.penalty_kick_angle, - 2, - self.kick_length, - self.angular_range) + -self.penalty_kick_angle, self.penalty_kick_angle, 2, self.kick_length, self.angular_range + ) else: ball_u, ball_v = self.blackboard.world_model.get_ball_position_uv() goal.kick_speed = 1.0 @@ -84,11 +86,12 @@ def perform(self, reevaluate=False): goal.unstable = False kick_direction = self.blackboard.costmap.get_best_kick_direction( - -self.max_kick_angle, - self.max_kick_angle, - self.num_kick_angles, - self.kick_length, - self.angular_range) + -self.max_kick_angle, + self.max_kick_angle, + self.num_kick_angles, + self.kick_length, + self.angular_range, + ) goal.kick_direction = quat_from_yaw(kick_direction) diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/play_animation.py b/bitbots_body_behavior/bitbots_body_behavior/actions/play_animation.py index c1c54573..8bff34fb 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/play_animation.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/play_animation.py @@ -1,9 +1,9 @@ -from abc import abstractmethod, ABC - -from bitbots_blackboard.blackboard import BodyBlackboard +from abc import ABC, abstractmethod from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard + class AbstractPlayAnimation(AbstractActionElement, ABC): """ @@ -11,7 +11,7 @@ class AbstractPlayAnimation(AbstractActionElement, ABC): """ def __init__(self, blackboard, dsd, parameters=None): - super(AbstractPlayAnimation, self).__init__(blackboard, dsd, parameters=None) + super().__init__(blackboard, dsd, parameters=None) self.blackboard: BodyBlackboard self.first_perform = True @@ -63,6 +63,7 @@ def get_animation_name(self): self.blackboard.node.get_logger().info("PLAYING GOALIE FALLING LEFT ANIMATION") return self.blackboard.animation.goalie_falling_left_animation + class PlayAnimationGoalieFallCenter(AbstractPlayAnimation): def get_animation_name(self): self.blackboard.node.get_logger().info("PLAYING GOALIE FALLING CENTER ANIMATION") diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/stand.py b/bitbots_body_behavior/bitbots_body_behavior/actions/stand.py index 817010b8..dd3f79a5 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/stand.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/stand.py @@ -1,10 +1,10 @@ import random +from dynamic_stack_decider.abstract_action_element import AbstractActionElement from geometry_msgs.msg import Twist from rclpy.duration import Duration from bitbots_blackboard.blackboard import BodyBlackboard -from dynamic_stack_decider.abstract_action_element import AbstractActionElement class CancelPathplanning(AbstractActionElement): @@ -22,7 +22,7 @@ class WalkInPlace(AbstractActionElement): def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) self.blackboard: BodyBlackboard - self.duration = parameters.get('duration', None) + self.duration = parameters.get("duration", None) self.start_time = self.blackboard.node.get_clock().now() @@ -31,7 +31,9 @@ def __init__(self, blackboard, dsd, parameters=None): def perform(self, reevaluate=False): self.publish_debug_data("duration", self.duration) - if self.duration is not None and (self.blackboard.node.get_clock().now() - self.start_time) >= Duration(seconds=self.duration): + if self.duration is not None and (self.blackboard.node.get_clock().now() - self.start_time) >= Duration( + seconds=self.duration + ): return self.pop() self.blackboard.pathfinding.direct_cmd_vel_pub.publish(Twist()) @@ -47,7 +49,9 @@ def __init__(self, blackboard, dsd, parameters=None): def perform(self, reevaluate=False): self.publish_debug_data("duration", self.duration) - if self.duration is not None and (self.blackboard.node.get_clock().now() - self.start_time) >= Duration(seconds=self.duration): + if self.duration is not None and (self.blackboard.node.get_clock().now() - self.start_time) >= Duration( + seconds=self.duration + ): return self.pop() # need to keep publishing this since path planning publishes a few more messages self.blackboard.pathfinding.stop_walk() @@ -58,8 +62,8 @@ class StandAndWaitRandom(Stand): def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) - self.min = parameters.get('min', None) - self.max = parameters.get('max', None) + self.min = parameters.get("min", None) + self.max = parameters.get("max", None) self.duration = random.uniform(self.min, self.max) self.start_time = self.blackboard.node.get_clock().now() diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/timer.py b/bitbots_body_behavior/bitbots_body_behavior/actions/timer.py index 1519d258..23c990ab 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/timer.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/timer.py @@ -1,19 +1,19 @@ -from bitbots_blackboard.blackboard import BodyBlackboard - from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from bitbots_blackboard.blackboard import BodyBlackboard + class StartTimer(AbstractActionElement): def __init__(self, blackboard, dsd, parameters=None): - super(StartTimer, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) self.blackboard: BodyBlackboard - if 'name' not in parameters: - raise KeyError('StartTimer: Name parameter is missing!') - self.timer_name = parameters['name'] - if 'duration' not in parameters: - raise KeyError(f'StartTimer ({self.timer_name}): Duration parameter is missing!') - self.duration = parameters['duration'] + if "name" not in parameters: + raise KeyError("StartTimer: Name parameter is missing!") + self.timer_name = parameters["name"] + if "duration" not in parameters: + raise KeyError(f"StartTimer ({self.timer_name}): Duration parameter is missing!") + self.duration = parameters["duration"] def perform(self, reevaluate=False): self.blackboard.misc.start_timer(self.timer_name, self.duration) @@ -22,12 +22,12 @@ def perform(self, reevaluate=False): class EndTimer(AbstractActionElement): def __init__(self, blackboard, dsd, parameters=None): - super(EndTimer, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) self.blackboard: BodyBlackboard - if 'name' not in parameters: - raise KeyError('EndTimer: Name parameter is missing!') - self.timer_name = parameters['name'] + if "name" not in parameters: + raise KeyError("EndTimer: Name parameter is missing!") + self.timer_name = parameters["name"] def perform(self, reevaluate=False): self.blackboard.misc.end_timer(self.timer_name) diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/turn.py b/bitbots_body_behavior/bitbots_body_behavior/actions/turn.py index 5f5422c6..950c87d1 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/turn.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/turn.py @@ -1,12 +1,11 @@ import math from typing import Optional -from bitbots_blackboard.blackboard import BodyBlackboard -from geometry_msgs.msg import PoseStamped, Twist -from geometry_msgs.msg import Quaternion - from bitbots_utils.transforms import quat_from_yaw from dynamic_stack_decider.abstract_action_element import AbstractActionElement +from geometry_msgs.msg import PoseStamped, Twist + +from bitbots_blackboard.blackboard import BodyBlackboard class TurnAround(AbstractActionElement): @@ -14,7 +13,7 @@ def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) self.blackboard: BodyBlackboard - self.orientation_thresh = parameters.get('thresh', 0.5) + self.orientation_thresh = parameters.get("thresh", 0.5) pose = self.blackboard.world_model.get_current_position() if pose is None: @@ -26,7 +25,7 @@ def __init__(self, blackboard, dsd, parameters=None): self.pose_msg = self.create_pose_msg(self.blackboard.map_frame, x, y, self.theta) - def create_pose_msg(self ,frame, x, y, theta): + def create_pose_msg(self, frame, x, y, theta): pose_msg = PoseStamped() pose_msg.header.stamp = self.blackboard.node.get_clock().now().to_msg() pose_msg.header.frame_id = frame @@ -53,14 +52,15 @@ class Turn(AbstractActionElement): Turns with up to max speed for a given duration. The sign of max speed indicates the direction (positive = left, negative = right). """ + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) self.blackboard: BodyBlackboard self.current_rotation_vel = 0.0 - self.max_speed = parameters.get('max_speed', 0.3) + self.max_speed = parameters.get("max_speed", 0.3) # Check if the have a duration - self.duration: Optional[float] = parameters.get('duration', None) + self.duration: Optional[float] = parameters.get("duration", None) self.start_time = self.blackboard.node.get_clock().now() # Cancel the path planning if it is running diff --git a/bitbots_body_behavior/bitbots_body_behavior/actions/walk.py b/bitbots_body_behavior/bitbots_body_behavior/actions/walk.py index 1d8fa219..bfa34c56 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/actions/walk.py +++ b/bitbots_body_behavior/bitbots_body_behavior/actions/walk.py @@ -1,7 +1,6 @@ -from rclpy.duration import Duration -from geometry_msgs.msg import Twist from dynamic_stack_decider.abstract_action_element import AbstractActionElement - +from geometry_msgs.msg import Twist +from rclpy.duration import Duration class WalkForward(AbstractActionElement): diff --git a/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py b/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py index 00e162c3..ab5cfecd 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py +++ b/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py @@ -10,20 +10,20 @@ import os -import tf2_ros as tf2 import rclpy +import tf2_ros as tf2 from ament_index_python import get_package_share_directory -from bitbots_blackboard.blackboard import BodyBlackboard +from bitbots_msgs.msg import GameState, RobotControlState, TeamData +from bitbots_tf_listener import TransformListener +from dynamic_stack_decider.dsd import DSD from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, TwistWithCovarianceStamped from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.duration import Duration from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node from soccer_vision_3d_msgs.msg import RobotArray -from bitbots_tf_listener import TransformListener -from dynamic_stack_decider.dsd import DSD -from bitbots_msgs.msg import GameState, RobotControlState, TeamData +from bitbots_blackboard.blackboard import BodyBlackboard class BodyDSD: diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/aligned_to_goal.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/aligned_to_goal.py index c254ab23..8d1722f1 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/aligned_to_goal.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/aligned_to_goal.py @@ -1,19 +1,19 @@ import math -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class AlignedToGoal(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(AlignedToGoal, self).__init__(blackboard, dsd, parameters) - self.goalpost_safety_distance = self.blackboard.config['goalpost_safety_distance'] + super().__init__(blackboard, dsd, parameters) + self.goalpost_safety_distance = self.blackboard.config["goalpost_safety_distance"] self.field_length = self.blackboard.world_model.field_length self.goal_width = self.blackboard.world_model.goal_width - self.max_kick_angle = self.blackboard.config['max_kick_angle'] + self.max_kick_angle = self.blackboard.config["max_kick_angle"] def perform(self, reevaluate=False): """ @@ -25,7 +25,7 @@ def perform(self, reevaluate=False): # Check if we know our position if current_pose is None: - return 'NO' + return "NO" # Get maximum kick angle relative to our base footprint angle_difference_right = current_pose[2] - self.max_kick_angle @@ -33,14 +33,18 @@ def perform(self, reevaluate=False): # Calculate the intersection of the left most kick with the goal line right of the center of the goal. dist_left represents the y coordinate of the intersection. if math.cos(angle_difference_left) > 0: - dist_left = current_pose[1] + (math.sin(angle_difference_left)) * (( -current_pose[0] + self.field_length / 2) / (math.cos(angle_difference_left))) + dist_left = current_pose[1] + (math.sin(angle_difference_left)) * ( + (-current_pose[0] + self.field_length / 2) / (math.cos(angle_difference_left)) + ) else: # Set None if no intersection can be found in the desired direction dist_left = None # Do this also for the right most kick in a similar way if math.cos(angle_difference_right) > 0: - dist_right = current_pose[1] + (math.sin(angle_difference_right)) * (( -current_pose[0] + self.field_length / 2) / (math.cos(angle_difference_right))) + dist_right = current_pose[1] + (math.sin(angle_difference_right)) * ( + (-current_pose[0] + self.field_length / 2) / (math.cos(angle_difference_right)) + ) else: # Set None if no intersection can be found in the desired direction dist_right = None diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/avoid_ball.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/avoid_ball.py index b809c62a..8e5c3e12 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/avoid_ball.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/avoid_ball.py @@ -1,13 +1,13 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class AvoidBall(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(AvoidBall, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) def perform(self, reevaluate=False): """ @@ -18,8 +18,8 @@ def perform(self, reevaluate=False): self.publish_debug_data("avoid_ball", self.blackboard.pathfinding.avoid_ball) if self.blackboard.pathfinding.avoid_ball: - return 'YES' - return 'NO' + return "YES" + return "NO" def get_reevaluate(self): return True diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_close.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_close.py index ed9284f9..ca0dfb6f 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_close.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_close.py @@ -1,16 +1,16 @@ import math -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class BallClose(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(BallClose, self).__init__(blackboard, dsd, parameters) - self.ball_close_distance = parameters.get("distance", self.blackboard.config['ball_close_distance']) + super().__init__(blackboard, dsd, parameters) + self.ball_close_distance = parameters.get("distance", self.blackboard.config["ball_close_distance"]) self.ball_close_angle = parameters.get("angle", math.pi) def perform(self, reevaluate=False): @@ -22,10 +22,12 @@ def perform(self, reevaluate=False): self.publish_debug_data("ball_distance", self.blackboard.world_model.get_ball_distance()) self.publish_debug_data("ball_angle", self.blackboard.world_model.get_ball_angle()) - if self.blackboard.world_model.get_ball_distance() < self.ball_close_distance and \ - abs(self.blackboard.world_model.get_ball_angle()) < self.ball_close_angle: - return 'YES' - return 'NO' + if ( + self.blackboard.world_model.get_ball_distance() < self.ball_close_distance + and abs(self.blackboard.world_model.get_ball_angle()) < self.ball_close_angle + ): + return "YES" + return "NO" def get_reevaluate(self): return True diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_dangerous.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_dangerous.py index b77cb151..2a40dac1 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_dangerous.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_dangerous.py @@ -1,19 +1,19 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class BallDangerous(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(BallDangerous, self).__init__(blackboard, dsd, parameters) - self.goal_radius = parameters.get("radius", self.blackboard.config['ball_dangerous_goal_radius']) - self.center_width = self.blackboard.config['ball_dangerous_center'] + super().__init__(blackboard, dsd, parameters) + self.goal_radius = parameters.get("radius", self.blackboard.config["ball_dangerous_goal_radius"]) + self.center_width = self.blackboard.config["ball_dangerous_center"] self.decided = False def perform(self, reevaluate=False): - """" + """ " Determines whether the position is in the dangerous area (in a radius close to the goal) """ ball_position = self.blackboard.world_model.get_ball_position_xy() @@ -21,14 +21,14 @@ def perform(self, reevaluate=False): self.decided = True robot_position = self.blackboard.world_model.get_current_position() if ball_position[1] > robot_position[1] + self.center_width / 2: - return 'LEFT' + return "LEFT" elif ball_position[1] < robot_position[1] - self.center_width / 2: - return 'RIGHT' - return 'CENTER' - return 'NO' + return "RIGHT" + return "CENTER" + return "NO" def _in_dangerous_area(self, position): - """" + """ " returns whether the position is in the dangerous area (close to the goal) """ diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_in_defensive_area.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_in_defensive_area.py index 387e783c..aaa2ec4f 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_in_defensive_area.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_in_defensive_area.py @@ -1,14 +1,14 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class BallInDefensiveArea(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(BallInDefensiveArea, self).__init__(blackboard, dsd, parameters) - self.defensive_area = self.blackboard.config['defensive_area'] + super().__init__(blackboard, dsd, parameters) + self.defensive_area = self.blackboard.config["defensive_area"] def perform(self, reevaluate=False): """ @@ -18,10 +18,12 @@ def perform(self, reevaluate=False): """ ball_position = self.blackboard.world_model.get_ball_position_xy() # calculate the x value of the boundary of the defensive area - defensive_x = (self.defensive_area * self.blackboard.world_model.field_length) - (self.blackboard.world_model.field_length / 2) + defensive_x = (self.defensive_area * self.blackboard.world_model.field_length) - ( + self.blackboard.world_model.field_length / 2 + ) if ball_position[0] <= defensive_x: - return 'YES' - return 'NO' + return "YES" + return "NO" def get_reevaluate(self): return True @@ -29,8 +31,9 @@ def get_reevaluate(self): class BallInOwnPercent(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(BallInOwnPercent, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) self.percent = parameters["p"] def perform(self, reevaluate=False): @@ -41,10 +44,12 @@ def perform(self, reevaluate=False): """ ball_position = self.blackboard.world_model.get_ball_position_xy() # calculate the x value of the boundary of the defensive area - defensive_x = ((self.percent / 100.0) * self.blackboard.world_model.field_length) - (self.blackboard.world_model.field_length / 2.0) + defensive_x = ((self.percent / 100.0) * self.blackboard.world_model.field_length) - ( + self.blackboard.world_model.field_length / 2.0 + ) if ball_position[0] <= defensive_x: - return 'YES' - return 'NO' + return "YES" + return "NO" def get_reevaluate(self): - return True \ No newline at end of file + return True diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_kick_area.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_kick_area.py index bd0258ce..0cbdef16 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_kick_area.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_kick_area.py @@ -1,19 +1,19 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class BallKickArea(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(BallKickArea, self).__init__(blackboard, dsd, parameters) - self.kick_x_enter = self.blackboard.config['kick_x_enter'] - self.kick_y_enter = self.blackboard.config['kick_y_enter'] - self.kick_x_leave = self.blackboard.config['kick_x_leave'] - self.kick_y_leave = self.blackboard.config['kick_y_leave'] + super().__init__(blackboard, dsd, parameters) + self.kick_x_enter = self.blackboard.config["kick_x_enter"] + self.kick_y_enter = self.blackboard.config["kick_y_enter"] + self.kick_x_leave = self.blackboard.config["kick_x_leave"] + self.kick_y_leave = self.blackboard.config["kick_y_leave"] self.last_descision = "FAR" - self.smoothing = self.blackboard.config['kick_decision_smoothing'] + self.smoothing = self.blackboard.config["kick_decision_smoothing"] self.no_near_decisions = 0 def perform(self, reevaluate=False): @@ -29,7 +29,7 @@ def perform(self, reevaluate=False): # 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: - self.last_descision = 'NEAR' + self.last_descision = "NEAR" self.no_near_decisions += 1 # Check if the ball is in the area between the enter area and the leave area elif 0 <= ball_position[0] <= self.kick_x_leave and 0 <= abs(ball_position[1]) <= self.kick_y_leave: @@ -39,16 +39,18 @@ def perform(self, reevaluate=False): elif self.last_descision == "NEAR": self.no_near_decisions += 1 else: - self.blackboard.node.get_logger().error(f"Unknown BallKickArea last return value: {self.last_descision}") + self.blackboard.node.get_logger().error( + f"Unknown BallKickArea last return value: {self.last_descision}" + ) # We are outside of both areas else: - self.last_descision = 'FAR' + self.last_descision = "FAR" self.no_near_decisions = 0 if self.no_near_decisions >= self.smoothing: - return 'NEAR' + return "NEAR" else: - return 'FAR' + return "FAR" def get_reevaluate(self): """ diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_seen.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_seen.py index 62bf292a..da6f4215 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_seen.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/ball_seen.py @@ -1,15 +1,15 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement from rclpy.duration import Duration -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class BallSeen(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(BallSeen, self).__init__(blackboard, dsd, parameters) - self.ball_lost_time = Duration(seconds=self.blackboard.config['ball_lost_time']) + super().__init__(blackboard, dsd, parameters) + self.ball_lost_time = Duration(seconds=self.blackboard.config["ball_lost_time"]) def perform(self, reevaluate=False): """ @@ -17,11 +17,12 @@ def perform(self, reevaluate=False): :param reevaluate: :return: """ - self.publish_debug_data("Ball lost time", - self.blackboard.node.get_clock().now() - self.blackboard.world_model.ball_last_seen()) + self.publish_debug_data( + "Ball lost time", self.blackboard.node.get_clock().now() - self.blackboard.world_model.ball_last_seen() + ) if self.blackboard.node.get_clock().now() - self.blackboard.world_model.ball_last_seen() < self.ball_lost_time: - return 'YES' - return 'NO' + return "YES" + return "NO" def get_reevaluate(self): return True diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/closest_to_ball.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/closest_to_ball.py index 303cb696..4b02242f 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/closest_to_ball.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/closest_to_ball.py @@ -1,19 +1,19 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class ClosestToBallNoGoalie(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(ClosestToBallNoGoalie, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) def perform(self, reevaluate=False): my_time_to_ball = self.blackboard.team_data.get_own_time_to_ball() rank = self.blackboard.team_data.team_rank_to_ball(my_time_to_ball, count_goalies=False, use_time_to_ball=True) - self.publish_debug_data(f"time to ball", my_time_to_ball) - self.publish_debug_data(f"Rank to ball", rank) + self.publish_debug_data("time to ball", my_time_to_ball) + self.publish_debug_data("Rank to ball", rank) if rank == 1: return "YES" return "NO" @@ -24,14 +24,15 @@ def get_reevaluate(self): class ClosestToBall(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(ClosestToBall, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) def perform(self, reevaluate=False): my_time_to_ball = self.blackboard.team_data.get_own_time_to_ball() rank = self.blackboard.team_data.team_rank_to_ball(my_time_to_ball, count_goalies=True, use_time_to_ball=True) - self.publish_debug_data(f"time to ball", my_time_to_ball) - self.publish_debug_data(f"Rank to ball", rank) + self.publish_debug_data("time to ball", my_time_to_ball) + self.publish_debug_data("Rank to ball", rank) if rank == 1: return "YES" return "NO" @@ -42,14 +43,15 @@ def get_reevaluate(self): class RankToBallNoGoalie(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) def perform(self, reevaluate=False): my_time_to_ball = self.blackboard.team_data.get_own_time_to_ball() rank = self.blackboard.team_data.team_rank_to_ball(my_time_to_ball, count_goalies=False, use_time_to_ball=True) - self.publish_debug_data(f"time to ball", my_time_to_ball) - self.publish_debug_data(f"Rank to ball", rank) + self.publish_debug_data("time to ball", my_time_to_ball) + self.publish_debug_data("Rank to ball", rank) if rank == 1: return "FIRST" elif rank == 2: diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/config_role.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/config_role.py index 31f9efd8..6c4aec34 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/config_role.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/config_role.py @@ -1,21 +1,21 @@ -# -*- coding:utf-8 -*- -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class ConfigRole(AbstractDecisionElement): """ Decides what kind of behaviour the robot performs """ + blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) self.role = self.blackboard.team_data.role def perform(self, reevaluate=False): - assert self.role in self.blackboard.config['roles'], "No valid role specified" + assert self.role in self.blackboard.config["roles"], "No valid role specified" return self.role.upper() def get_reevaluate(self): diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/current_score.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/current_score.py index 4515a98b..9ca92330 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/current_score.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/current_score.py @@ -1,11 +1,11 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class CurrentScore(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/do_once.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/do_once.py index a8869cdf..6b1c3cbd 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/do_once.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/do_once.py @@ -1,5 +1,4 @@ -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement class DoOnce(AbstractDecisionElement): @@ -9,10 +8,10 @@ def __init__(self, blackboard, dsd, parameters=None): def perform(self, reevaluate=False): if self.done: - return 'DONE' + return "DONE" else: self.done = True - return 'NOT_DONE' + return "NOT_DONE" def get_reevaluate(self): return False diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/dribble_or_kick.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/dribble_or_kick.py index 3fb8d1f8..3eaf07bf 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/dribble_or_kick.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/dribble_or_kick.py @@ -1,23 +1,23 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class DribbleOrKick(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) - self.orient_threshold = self.blackboard.config['dribble_orient_threshold'] - self.goal_distance_threshold = self.blackboard.config['dribble_goal_distance_threshold'] - self.ball_distance_threshold = self.blackboard.config['dribble_ball_distance_threshold'] + self.orient_threshold = self.blackboard.config["dribble_orient_threshold"] + self.goal_distance_threshold = self.blackboard.config["dribble_goal_distance_threshold"] + self.ball_distance_threshold = self.blackboard.config["dribble_ball_distance_threshold"] - self.kick_length = self.blackboard.config['kick_cost_kick_length'] - self.max_kick_angle = self.blackboard.config['max_kick_angle'] - self.angular_range = self.blackboard.config['kick_cost_angular_range'] - self.num_kick_angles = self.blackboard.config['num_kick_angles'] + self.kick_length = self.blackboard.config["kick_cost_kick_length"] + self.max_kick_angle = self.blackboard.config["max_kick_angle"] + self.angular_range = self.blackboard.config["kick_cost_angular_range"] + self.num_kick_angles = self.blackboard.config["num_kick_angles"] - self.dribble_kick_angle = self.blackboard.config['dribble_kick_angle'] + self.dribble_kick_angle = self.blackboard.config["dribble_kick_angle"] def perform(self, reevaluate=False): """ @@ -31,18 +31,18 @@ def perform(self, reevaluate=False): self.publish_debug_data(f"Orientation to goal (needs <{self.orient_threshold})", goal_angle) # no other robots should be in front of the ball. this means the kick with angle 0 would be the best - best_kick_direction = self.blackboard.costmap.get_best_kick_direction(-self.max_kick_angle, - self.max_kick_angle, - self.num_kick_angles, - self.kick_length, - self.angular_range) + best_kick_direction = self.blackboard.costmap.get_best_kick_direction( + -self.max_kick_angle, self.max_kick_angle, self.num_kick_angles, self.kick_length, self.angular_range + ) front_free = -self.dribble_kick_angle < best_kick_direction < self.dribble_kick_angle self.publish_debug_data("best kick direction", best_kick_direction) self.publish_debug_data("Front free", front_free) # we should be not to close to the goal, otherwise kicking makes more sense. only take x axis into account - goal_distance = abs(self.blackboard.world_model.get_map_based_opp_goal_center_xy()[0] - - self.blackboard.world_model.get_current_position()[0]) + goal_distance = abs( + self.blackboard.world_model.get_map_based_opp_goal_center_xy()[0] + - self.blackboard.world_model.get_current_position()[0] + ) goal_far = goal_distance > self.goal_distance_threshold self.publish_debug_data(f"Goal distance (needs >{self.goal_distance_threshold})", goal_distance) diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/game_state_decider.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/game_state_decider.py index 877377ee..44b3ee19 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/game_state_decider.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/game_state_decider.py @@ -1,14 +1,14 @@ -from bitbots_blackboard.blackboard import BodyBlackboard - -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement from bitbots_msgs.msg import GameState +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement + +from bitbots_blackboard.blackboard import BodyBlackboard class GameStateDecider(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(GameStateDecider, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) def perform(self, reevaluate=False): """ @@ -18,7 +18,7 @@ def perform(self, reevaluate=False): """ game_state_number = self.blackboard.gamestate.get_gamestate() - #todo this is a temporary hack to make GUI work + # todo this is a temporary hack to make GUI work if game_state_number == GameState.GAMESTATE_INITIAL: return "INITIAL" elif game_state_number == GameState.GAMESTATE_READY: diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/goal_scored.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/goal_scored.py index 5f45db41..b6fcd87c 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/goal_scored.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/goal_scored.py @@ -1,14 +1,14 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class GoalScoreRecently(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(GoalScoreRecently, self).__init__(blackboard, dsd, parameters) - self.time = parameters.get('time', 2) + super().__init__(blackboard, dsd, parameters) + self.time = parameters.get("time", 2) def perform(self, reevaluate=False): """ @@ -18,14 +18,16 @@ def perform(self, reevaluate=False): """ if self.blackboard.gamestate.get_seconds_since_own_goal() < self.time: - return 'YES' - return 'NO' + return "YES" + return "NO" def get_reevaluate(self): return True + class AnyGoalScoreRecently(GoalScoreRecently): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) @@ -37,8 +39,8 @@ def perform(self, reevaluate=False): """ if self.blackboard.gamestate.get_seconds_since_any_goal() < self.time: - return 'YES' - return 'NO' + return "YES" + return "NO" def get_reevaluate(self): return True diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/goalie_handling_ball.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/goalie_handling_ball.py index 5e5f1761..70049079 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/goalie_handling_ball.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/goalie_handling_ball.py @@ -1,11 +1,11 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class GoalieHandlingBall(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/is_penalized.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/is_penalized.py index a9eb87a9..3bca9fdc 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/is_penalized.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/is_penalized.py @@ -1,11 +1,11 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class IsPenalized(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) @@ -13,8 +13,7 @@ def perform(self, reevaluate=False): """ Determines if the robot is penalized by the game controller. """ - self.publish_debug_data("Seconds since unpenalized", - self.blackboard.gamestate.get_seconds_since_unpenalized()) + self.publish_debug_data("Seconds since unpenalized", self.blackboard.gamestate.get_seconds_since_unpenalized()) if self.blackboard.gamestate.get_is_penalized(): return "YES" elif self.blackboard.gamestate.get_seconds_since_unpenalized() < 1: diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/kick_off_time_up.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/kick_off_time_up.py index 090cc9ea..5d69b6fd 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/kick_off_time_up.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/kick_off_time_up.py @@ -1,11 +1,11 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class KickOffTimeUp(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) self.kickoff_min_ball_movement = self.blackboard.config["kickoff_min_ball_movement"] @@ -13,27 +13,29 @@ def __init__(self, blackboard, dsd, parameters=None): def perform(self, reevaluate=False): if self.blackboard.gamestate.has_kickoff(): self.publish_debug_data("Reason", "Our kick off") - return 'YES' + return "YES" else: if self.blackboard.gamestate.get_secondary_seconds_remaining() == 0: self.publish_debug_data("Reason", "Opp kick off time ended") # time is up for the other team - return 'YES' + return "YES" ball_pos = self.blackboard.world_model.get_ball_position_xy() # check if this is a normal kickoff if self.blackboard.gamestate.free_kick_kickoff_team is None: # if we know where the ball is and that it moved, we can play too - if abs(ball_pos[0]) > self.kickoff_min_ball_movement or \ - abs(ball_pos[1]) > self.kickoff_min_ball_movement: + if ( + abs(ball_pos[0]) > self.kickoff_min_ball_movement + or abs(ball_pos[1]) > self.kickoff_min_ball_movement + ): self.publish_debug_data("Reason", "Opp kick off ball moved") - return 'YES' + return "YES" else: # this is some kind of free kick and we may want to act differently self.publish_debug_data("Reason", "Opp has free kick") - return 'NO_FREEKICK' + return "NO_FREEKICK" self.publish_debug_data("Reason", "Opp has kick off") # we need to wait for now - return 'NO_NORMAL' + return "NO_NORMAL" def get_reevaluate(self): return True diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/last_player.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/last_player.py index 3d9f151e..aed79527 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/last_player.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/last_player.py @@ -1,11 +1,11 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class LastPlayer(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/pass_started.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/pass_started.py index acf92ce9..8e39d0d3 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/pass_started.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/pass_started.py @@ -1,11 +1,11 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class PassStarted(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): super().__init__(blackboard, dsd, parameters) diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/reached_goal.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/reached_goal.py index f9de5b98..c1b2c697 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/reached_goal.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/reached_goal.py @@ -1,18 +1,18 @@ import math -import numpy as np -from bitbots_blackboard.blackboard import BodyBlackboard -from tf_transformations import euler_from_quaternion +import numpy as np +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement from ros2_numpy import numpify +from tf_transformations import euler_from_quaternion -from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class ReachedPathPlanningGoalPosition(AbstractDecisionElement): blackboard: BodyBlackboard def __init__(self, blackboard, dsd, parameters=None): - super(ReachedPathPlanningGoalPosition, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) self.frame_id = parameters.get("frame_id", self.blackboard.map_frame) self.threshold = parameters.get("threshold") @@ -42,7 +42,7 @@ class AlignedToPathPlanningGoal(AbstractDecisionElement): blackboard: BodyBlackboard def __init__(self, blackboard, dsd, parameters=None): - super(AlignedToPathPlanningGoal, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) self.orientation_threshold = self.blackboard.config["goal_alignment_orientation_threshold"] # [deg] self.frame_id = parameters.get("frame_id", self.blackboard.map_frame) diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/secondary_state_decider.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/secondary_state_decider.py index 3c6d2f3f..c5f818db 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/secondary_state_decider.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/secondary_state_decider.py @@ -1,8 +1,7 @@ -from bitbots_blackboard.blackboard import BodyBlackboard - -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement from bitbots_msgs.msg import GameState +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement + +from bitbots_blackboard.blackboard import BodyBlackboard class SecondaryStateDecider(AbstractDecisionElement): @@ -10,9 +9,11 @@ class SecondaryStateDecider(AbstractDecisionElement): Decides in which secondary state the game is currently in. The mode of the secondary state is handled in the game controller receiver, so the behavior does ont need to deal with this. """ + blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(SecondaryStateDecider, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) def perform(self, reevaluate=False): state_number = self.blackboard.gamestate.get_secondary_state() @@ -49,9 +50,11 @@ class SecondaryStateTeamDecider(AbstractDecisionElement): """ Decides if our team or the other team is allowed to execute the secondary state. """ + blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(SecondaryStateTeamDecider, self).__init__(blackboard, dsd) + super().__init__(blackboard, dsd) self.team_id = self.blackboard.gamestate.get_team_id() def perform(self, reevaluate=False): @@ -59,12 +62,12 @@ def perform(self, reevaluate=False): # we have to handle penalty shoot differently because the message is strange if state_number == GameState.STATE_PENALTYSHOOT: if self.blackboard.gamestate.has_kickoff(): - return 'OUR' - return 'OTHER' + return "OUR" + return "OTHER" else: if self.blackboard.gamestate.get_secondary_team() == self.team_id: - return 'OUR' - return 'OTHER' + return "OUR" + return "OTHER" def get_reevaluate(self): """ @@ -77,18 +80,20 @@ class SecondaryStateModeDecider(AbstractDecisionElement): """ Decides which mode in the secondary state we are. """ + blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(SecondaryStateModeDecider, self).__init__(blackboard, dsd) + super().__init__(blackboard, dsd) def perform(self, reevaluate=False): state_mode = self.blackboard.gamestate.get_secondary_state_mode() if state_mode == GameState.MODE_PREPARATION: - return 'PREPARATION' + return "PREPARATION" elif state_mode == GameState.MODE_PLACING: - return 'PLACING' + return "PLACING" elif state_mode == GameState.MODE_END: - return 'END' + return "END" def get_reevaluate(self): return True diff --git a/bitbots_body_behavior/bitbots_body_behavior/decisions/timer.py b/bitbots_body_behavior/bitbots_body_behavior/decisions/timer.py index d7660033..470f0f12 100644 --- a/bitbots_body_behavior/bitbots_body_behavior/decisions/timer.py +++ b/bitbots_body_behavior/bitbots_body_behavior/decisions/timer.py @@ -1,17 +1,17 @@ -from bitbots_blackboard.blackboard import BodyBlackboard +from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement -from dynamic_stack_decider.abstract_decision_element import \ - AbstractDecisionElement +from bitbots_blackboard.blackboard import BodyBlackboard class TimerRunning(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(TimerRunning, self).__init__(blackboard, dsd, parameters) - if 'name' not in parameters: - raise KeyError('TimerRunning: Name parameter is missing!') - self.name = parameters['name'] - self.last_result = '' + super().__init__(blackboard, dsd, parameters) + if "name" not in parameters: + raise KeyError("TimerRunning: Name parameter is missing!") + self.name = parameters["name"] + self.last_result = "" def perform(self, reevaluate=False): """ @@ -21,9 +21,9 @@ def perform(self, reevaluate=False): """ self.publish_debug_data(f"timer {self.name}", self.blackboard.misc.timer_remaining(self.name)) if self.blackboard.misc.timer_running(self.name): - self.last_result = 'YES' + self.last_result = "YES" else: - self.last_result = 'NO' + self.last_result = "NO" return self.last_result def get_reevaluate(self): @@ -32,17 +32,18 @@ def get_reevaluate(self): This allows to define a situation in which the timer has ended after it was started before. :return: Whether the decision is reevaluated (When the last result was 'NO') """ - return self.last_result == 'NO' + return self.last_result == "NO" class TimerEnded(AbstractDecisionElement): blackboard: BodyBlackboard + def __init__(self, blackboard, dsd, parameters=None): - super(TimerEnded, self).__init__(blackboard, dsd, parameters) + super().__init__(blackboard, dsd, parameters) - if 'name' not in parameters: - raise KeyError('TimerEnded: Name parameter is missing!') - self.name = parameters['name'] + if "name" not in parameters: + raise KeyError("TimerEnded: Name parameter is missing!") + self.name = parameters["name"] def perform(self, reevaluate=False): """ @@ -53,8 +54,8 @@ def perform(self, reevaluate=False): self.publish_debug_data(f"timer {self.name}", self.blackboard.misc.timer_remaining(self.name)) if self.blackboard.misc.timer_ended(self.name): - return 'YES' - return 'NO' + return "YES" + return "NO" def get_reevaluate(self): - return True \ No newline at end of file + return True diff --git a/bitbots_body_behavior/docs/conf.py b/bitbots_body_behavior/docs/conf.py index 586c7734..0bf6043d 100644 --- a/bitbots_body_behavior/docs/conf.py +++ b/bitbots_body_behavior/docs/conf.py @@ -1,4 +1,3 @@ -# -*- coding: utf-8 -*- # # Full list of options at http://www.sphinx-doc.org/en/master/config @@ -10,28 +9,30 @@ # import os import sys -import catkin_pkg.package +import catkin_pkg.package from exhale import utils package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) catkin_package = catkin_pkg.package.parse_package( - os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) + os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME) +) sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) # -- Helper functions -------------------------------------------------------- + def count_files(): """:returns tuple of (num_py, num_cpp)""" num_py = 0 num_cpp = 0 - for root, dirs, files in os.walk(os.path.join(package_dir, "src")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "src")): for f in files: if f.endswith(".py"): num_py += 1 - for root, dirs, files in os.walk(os.path.join(package_dir, "include")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "include")): for f in files: if f.endswith(".h") or f.endswith(".hpp"): num_cpp += 1 @@ -42,7 +43,7 @@ def count_files(): # -- Project information ----------------------------------------------------- project = catkin_package.name -copyright = '2019, Bit-Bots' +copyright = "2019, Bit-Bots" author = ", ".join([a.name for a in catkin_package.authors]) # The short X.Y version @@ -60,27 +61,27 @@ def count_files(): # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.doctest', - 'sphinx.ext.intersphinx', - 'sphinx.ext.todo', - 'sphinx.ext.coverage', - 'sphinx.ext.imgmath', - 'sphinx.ext.viewcode', - 'sphinx_rtd_theme', + "sphinx.ext.autodoc", + "sphinx.ext.doctest", + "sphinx.ext.intersphinx", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.imgmath", + "sphinx.ext.viewcode", + "sphinx_rtd_theme", ] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: # # source_suffix = ['.rst', '.md'] -source_suffix = '.rst' +source_suffix = ".rst" # The master toctree document. -master_doc = 'index' +master_doc = "index" # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -92,7 +93,7 @@ def count_files(): # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # The name of the Pygments (syntax highlighting) style to use. pygments_style = None @@ -108,24 +109,17 @@ def count_files(): if num_files_cpp > 0: extensions += [ - 'breathe', - 'exhale', + "breathe", + "exhale", ] - breathe_projects = { - project: os.path.join("_build", "doxyoutput", "xml") - } + breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} breathe_default_project = project def specifications_for_kind(kind): # Show all members for classes and structs if kind == "class" or kind == "struct": - return [ - ":members:", - ":protected-members:", - ":private-members:", - ":undoc-members:" - ] + return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] # An empty list signals to Exhale to use the defaults else: return [] @@ -136,13 +130,11 @@ def specifications_for_kind(kind): "rootFileName": "library_root.rst", "rootFileTitle": "C++ Library API", "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping( - specifications_for_kind - ), + "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), # Suggested optional arguments "createTreeView": True, "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")) + "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), } # -- Options for HTML output ------------------------------------------------- @@ -150,7 +142,7 @@ def specifications_for_kind(kind): # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'sphinx_rtd_theme' +html_theme = "sphinx_rtd_theme" # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -161,7 +153,7 @@ def specifications_for_kind(kind): # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] # Custom sidebar templates, must be a dictionary that maps document names # to template names. @@ -173,14 +165,14 @@ def specifications_for_kind(kind): # # html_sidebars = {} -html_logo = os.path.join('_static', 'logo.png') -html_favicon = os.path.join('_static', 'logo.png') +html_logo = os.path.join("_static", "logo.png") +html_favicon = os.path.join("_static", "logo.png") # -- Options for intersphinx extension --------------------------------------- # Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {'https://docs.python.org/': None} +intersphinx_mapping = {"https://docs.python.org/": None} # -- Options for todo extension ---------------------------------------------- @@ -188,6 +180,8 @@ def specifications_for_kind(kind): todo_include_todos = True # -- RST Standard variables --------------------------------------------------- -rst_prolog = ".. |project| replace:: {}\n".format(project) +rst_prolog = f".. |project| replace:: {project}\n" rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available") +rst_prolog += ".. |modindex| replace:: {}\n".format( + ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" +) diff --git a/bitbots_body_behavior/setup.py b/bitbots_body_behavior/setup.py index 23368eb3..521b92d2 100644 --- a/bitbots_body_behavior/setup.py +++ b/bitbots_body_behavior/setup.py @@ -1,42 +1,34 @@ import glob -import os -from setuptools import find_packages from setuptools import setup -package_name = 'bitbots_body_behavior' +package_name = "bitbots_body_behavior" setup( name=package_name, packages=[package_name], data_files=[ - ('share/' + package_name, ['package.xml']), - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name + "/config", - glob.glob('config/*.yaml')), - ('share/' + package_name + '/launch', - glob.glob('launch/*.launch')), - ('share/' + package_name + '/actions', - glob.glob(package_name + '/actions/*.py')), - ('share/' + package_name + '/decisions' , - glob.glob(package_name + '/decisions/*.py')), - ('share/' + package_name, - glob.glob(package_name + '/*.dsd')), + ("share/" + package_name, ["package.xml"]), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name + "/config", glob.glob("config/*.yaml")), + ("share/" + package_name + "/launch", glob.glob("launch/*.launch")), + ("share/" + package_name + "/actions", glob.glob(package_name + "/actions/*.py")), + ("share/" + package_name + "/decisions", glob.glob(package_name + "/decisions/*.py")), + ("share/" + package_name, glob.glob(package_name + "/*.dsd")), ], scripts=[], install_requires=[ - 'launch', - 'setuptools', + "launch", + "setuptools", ], zip_safe=True, - keywords=['ROS'], - license='MIT', - tests_require=['pytest'], + keywords=["ROS"], + license="MIT", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - 'body_behavior = bitbots_body_behavior.body_behavior:main', + "console_scripts": [ + "body_behavior = bitbots_body_behavior.body_behavior:main", ], - } + }, ) diff --git a/bitbots_body_behavior/test/test_dsd_file.py b/bitbots_body_behavior/test/test_dsd_file.py index 9a9fe93d..6cbd9145 100755 --- a/bitbots_body_behavior/test/test_dsd_file.py +++ b/bitbots_body_behavior/test/test_dsd_file.py @@ -8,9 +8,9 @@ def test_dsd_valid(): # Create empty blackboard - DummyBlackboard = object + dummy_blackboard = object # Create DSD - dsd = DSD(DummyBlackboard()) + dsd = DSD(dummy_blackboard()) # Find install path of package where the dsd and python files are located dirname = get_package_share_directory("bitbots_body_behavior") From 84bebe3e9e81b2d329f37517ae7c04f512921be6 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Thu, 23 Nov 2023 00:44:53 +0100 Subject: [PATCH 4/5] chore(pre-commit): add `.git-blame-ignore-revs` to prevent full repo autoformatting to affect the git history, when utilizing `git blame` --- .git-blame-ignore-revs | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .git-blame-ignore-revs diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs new file mode 100644 index 00000000..066412d8 --- /dev/null +++ b/.git-blame-ignore-revs @@ -0,0 +1,2 @@ +# Add pre-commit auto formatting +31956156a6cab20c1f54f976dac745d95a1254c0 From a72d507df16122146e0b13d7eb4e3434f7185089 Mon Sep 17 00:00:00 2001 From: texhnolyze Date: Thu, 23 Nov 2023 00:54:05 +0100 Subject: [PATCH 5/5] refactor(pre-commit): adjust to template repo --- .github/workflows/pre-commit.yml | 2 +- .pre-commit-config.yaml | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index a7bea7fa..bb684803 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -1,4 +1,4 @@ -name: Code style checks with pre-commit +name: Code style checks on: pull_request: diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index f5e42d68..561c3bd3 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -19,4 +19,5 @@ repos: - id: cppcheck args: - "--suppress=missingInclude" - - "--suppress=missingIncludeSystem" + - "--suppress=unmatchedSuppression" + - "--suppress=unusedFunction"