Merge branch 'main' into feature/dynamic_reconfigure_dynup

.vscode/settings.json

@@ -16,6 +16,7 @@
         "colcon",
         "cornerkick",
         "costmap",
+        "costmaps",
         "cpus",
         "cuda",
         "cudnn",
@@ -27,6 +28,7 @@
         "dtype",
         "dynamixel",
         "dynup",
+        "euler",
         "externproto",
         "fetchrobotics",
         "fieldboundary",
@@ -35,6 +37,7 @@
         "freekick",
         "gamecontroller",
         "gamestate",
+        "gaussian",
         "goalkick",
         "hexsha",
         "hlvs",
@@ -63,6 +66,7 @@
         "odom",
         "odometry",
         "particlefilter",
+        "pathfinding",
         "penaltykick",
         "penaltyshoot",
         "pointcloud",
@@ -209,4 +213,7 @@
     "editor.formatOnSave": true,
     "editor.defaultFormatter": "charliermarsh.ruff",
     "restructuredtext.pythonRecommendation.disabled": true,
+    "[xml]": {
+        "editor.defaultFormatter": "DotJoshJohnson.xml"
+    },
 }

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/animation_capsule.py

@@ -1,9 +1,3 @@
-"""
-AnimationCapsule
-^^^^^^^^^^^^^^^^
-
-Communicates with the animation action server and plays predefined animations.
-"""
 from rclpy.action import ActionClient
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.duration import Duration
@@ -13,6 +7,8 @@
 
 
 class AnimationCapsule:
+    """Communicates with the animation action server to play animations."""
+
     def __init__(self, node: Node):
         self.node = node
         self.active = False

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py

@@ -1,9 +1,3 @@
-"""
-CostmapCapsule
-^^^^^^^^^^^^^^^^^^
-
-Provides information about the cost of different positions and moves.
-"""
 import math
 from typing import TYPE_CHECKING, List, Optional, Tuple
 
@@ -28,6 +22,8 @@
 
 
 class CostmapCapsule:
+    """Provides information about the cost of different positions and moves."""
+
     def __init__(self, blackboard: "BodyBlackboard"):
         self._blackboard = blackboard
         self._node = blackboard.node
@@ -137,11 +133,11 @@ def get_pass_regions(self) -> np.ndarray:
 
     def field_2_costmap_coord(self, x: float, y: float) -> Tuple[float, float]:
         """
-        Converts a field position to the coresponding indices for the costmap.
+        Converts a field position to the corresponding indices for the costmap.
 
         :param x: X Position relative to the center point. (Positive is towards the enemy goal)
         :param y: Y Position relative to the center point. (Positive is towards the left when we face the enemy goal)
-        :return: The x index of the coresponding costmap slot, The y index of the coresponding costmap slot
+        :return: The x index of the corresponding costmap slot, The y index of the corresponding costmap slot
         """
         idx_x = int(
             min(
@@ -192,7 +188,7 @@ def cost_at_relative_xy(self, x: float, y: float) -> float:
 
     def calc_base_costmap(self):
         """
-        Builds the base costmap based on the bahavior parameters.
+        Builds the base costmap based on the behavior parameters.
         This costmap includes a gradient towards the enemy goal and high costs outside the playable area
         """
         # Get parameters
@@ -265,23 +261,23 @@ def calc_base_costmap(self):
             ]
         )
 
-        # Apply map margin to fixpoints
+        # Apply map margin to fix points
         fix_points = [((p[0][0] + self.map_margin, p[0][1] + self.map_margin), p[1]) for p in fix_points]
 
-        # Interpolate the keypoints from above to form the costmap
+        # Interpolate the key points from above to form the costmap
         interpolated = griddata(
             [p[0] for p in fix_points], [p[1] for p in fix_points], (grid_x, grid_y), method="linear"
         )
 
-        # Smooth the costmap to get more continus gradients
+        # Smooth the costmap to get more continuous gradients
         self.base_costmap = gaussian_filter(interpolated, self.body_config["base_costmap_smoothing_sigma"])
         self.costmap = self.base_costmap.copy()
 
     def get_gradient_at_field_position(self, x: float, y: float) -> Tuple[float, float]:
         """
         Gets the gradient tuple at a given field position
-        :param x: Field coordiante in the x direction
-        :param y: Field coordiante in the y direction
+        :param x: Field coordinate in the x direction
+        :param y: Field coordinate in the y direction
         """
         idx_x, idx_y = self.field_2_costmap_coord(x, y)
         return -self.gradient_map[0][idx_x, idx_y], -self.gradient_map[1][idx_x, idx_y]
@@ -298,8 +294,8 @@ def get_cost_at_field_position(self, x: float, y: float) -> float:
     def get_gradient_direction_at_field_position(self, x: float, y: float):
         """
         Returns the gradient direction at the given position
-        :param x: Field coordiante in the x direction
-        :param y: Field coordiante in the y direction
+        :param x: Field coordinate in the x direction
+        :param y: Field coordinate in the y direction
         """
         # for debugging only
         # if False and self.costmap.sum() > 0:
@@ -317,8 +313,8 @@ def get_gradient_direction_at_field_position(self, x: float, y: float):
     def get_cost_of_kick_relative(self, x: float, y: float, direction: float, kick_length: float, angular_range: float):
         """
         Returns the cost of a kick at the given position and direction in base footprint frame
-        :param x: Field coordiante in the x direction
-        :param y: Field coordiante in the y direction
+        :param x: Field coordinate in the x direction
+        :param y: Field coordinate in the y direction
         :param direction: The direction of the kick
         :param kick_length: The length of the kick
         :param angular_range: The angular range of the kick"""
@@ -346,8 +342,8 @@ def get_cost_of_kick_relative(self, x: float, y: float, direction: float, kick_l
     def get_cost_of_kick(self, x: float, y: float, direction: float, kick_length: float, angular_range: float) -> float:
         """
         Returns the cost of the kick at the given position
-        :param x: Field coordiante in the x direction
-        :param y: Field coordiante in the y direction
+        :param x: Field coordinate in the x direction
+        :param y: Field coordinate in the y direction
         :param direction: The direction of the kick
         :param kick_length: The length of the kick
         :param angular_range: The angular range of the kick
@@ -378,7 +374,7 @@ def get_cost_of_kick(self, x: float, y: float, direction: float, kick_length: fl
         # plt.show()
 
         # The main influence should be the maximum cost in the area which is covered by the kick. This could be the field boundary, robots, ...
-        # But we also want prio directions with lower min cost. This could be the goal area or the pass accept area of an teammate
+        # But we also want prioritize directions with lower min cost. This could be the goal area or the pass accept area of an teammate
         # This should contribute way less than the max and should have an impact if the max values are similar in all directions.
         return masked_costmap.max() * 0.75 + masked_costmap.min() * 0.25
 

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py

@@ -1,15 +1,11 @@
-"""
-GameStatusCapsule
-^^^^^^^^^^^^^^^^^
-
-Provides information about the current game state.
-"""
 from bitbots_utils.utils import get_parameters_from_other_node
 from game_controller_hl_interfaces.msg import GameState
 from rclpy.node import Node
 
 
 class GameStatusCapsule:
+    """Provides information about the current game state."""
+
     def __init__(self, node: Node):
         self.node = node
         self.team_id = get_parameters_from_other_node(self.node, "parameter_blackboard", ["team_id"])["team_id"]

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/kick_capsule.py

@@ -1,9 +1,3 @@
-"""
-KickCapsule
-^^^^^^^^^^^^^^^^
-
-Communicates with the dynamic_kick.
-"""
 from typing import TYPE_CHECKING, Optional
 
 if TYPE_CHECKING:
@@ -18,6 +12,8 @@
 
 
 class KickCapsule:
+    """Communicates with the dynamic_kick action server to kick the ball."""
+
     __blackboard: "BodyBlackboard"
 
     last_feedback: Optional[Kick.Feedback] = None

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py

@@ -1,9 +1,3 @@
-"""
-MiscCapsule
-^^^^^^^^^^^
-
-Capsule for miscellaneous things that don't fit anywhere else.
-"""
 from typing import Optional
 
 from bitbots_utils.utils import get_parameters_from_other_node
@@ -15,6 +9,8 @@
 
 
 class MiscCapsule:
+    """Capsule for miscellaneous functions."""
+
     def __init__(self, node: Node):
         self.node = node
         self.head_pub = node.create_publisher(HeadMode, "head_mode", 10)

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py

@@ -1,10 +1,3 @@
-"""
-PathfindingCapsule
-^^^^^^^^^^^
-
-Capsule that handles everything related to pathfinding.
-"""
-
 import math
 from enum import Enum
 from typing import TYPE_CHECKING, Optional
@@ -31,6 +24,8 @@ class BallGoalType(Enum):
 
 
 class PathfindingCapsule:
+    """Capsule for pathfinding related functions."""
+
     def __init__(self, blackboard: "BodyBlackboard", node: Node):
         self.node = node
         self._blackboard = blackboard

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py

@@ -1,7 +1,3 @@
-"""
-TeamDataCapsule
-^^^^^^^^^^^^^^^
-"""
 from typing import Dict, List, Optional, Tuple
 
 import numpy as np
@@ -19,6 +15,8 @@
 
 class TeamDataCapsule:
     def __init__(self, node: Node):
+        """Handles incoming team data communication."""
+
         self.node = node
 
         # Publishers

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py

@@ -1,10 +1,3 @@
-"""
-WorldModelCapsule
-^^^^^^^^^^^^^^^^^^
-
-Provides information about the world model.
-"""
-
 import math
 from typing import TYPE_CHECKING, Dict, Optional, Tuple
 
@@ -32,6 +25,8 @@
 
 
 class WorldModelCapsule:
+    """Provides information about the world model."""
+
     def __init__(self, blackboard: "BodyBlackboard"):
         self._blackboard = blackboard
         self.body_config = get_parameter_dict(self._blackboard.node, "body")

bitbots_behavior/bitbots_blackboard/package.xml

@@ -20,7 +20,6 @@
 
     <author email="info@bit-bots.de">Hamburg Bit-Bots</author>
 
-
     <build_depend>bitbots_docs</build_depend>
     <depend>bitbots_tf_listener</depend>
     <depend>bitbots_utils</depend>
@@ -44,6 +43,6 @@
             <status>unknown</status>
             <language>python3</language>
         </bitbots_documentation>
-    <build_type>ament_cmake</build_type>
-  </export>
+        <build_type>ament_cmake</build_type>
+    </export>
 </package>

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py

@@ -1,13 +1,3 @@
-#!/usr/bin/env python3
-
-"""
-BehaviorModule
-^^^^^^^^^^^^^^
-.. moduleauthor:: Martin Poppinga <1popping@informatik.uni-hamburg.de>
-
-Starts the body behavior
-"""
-
 import os
 
 import rclpy

bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -148,7 +148,7 @@
       # Range in which the ball far approach point is counted as reached
       ball_far_approach_position_thresh: 0.2
 
-      # We reapproach the ball after it has moved further away than this distance. This includes moveing to the far approach position.
+      # We reapproach the ball after it has moved further away than this distance. This includes moving to the far approach position.
       ball_reapproach_dist: 1.0
 
       # Distance at which the ball is normally approached