Merge branch 'main' into feature/setup-script

.devcontainer/Dockerfile

@@ -57,12 +57,9 @@ RUN apt-get install -y \
   ros-iron-camera-calibration \
   ros-iron-desktop \
   ros-iron-joint-state-publisher-gui \
-  # ros-iron-plotjuggler-ros containing plotjuggler ros plugins
-  # build currently fails and is not available as a package so we
-  # have to manually install plotjuggler and plotjuggler-msgs
-  # https://github.com/PlotJuggler/plotjuggler-ros-plugins/issues/59
   ros-iron-plotjuggler \
   ros-iron-plotjuggler-msgs \
+  ros-iron-plotjuggler-ros \
   ros-iron-rmw-cyclonedds-cpp \
   ros-iron-rqt-robot-monitor \
   ros-iron-soccer-vision-3d-rviz-markers

.github/workflows/ci.yml

@@ -11,6 +11,10 @@ jobs:
       image: ubuntu:jammy
 
     steps:
+      - name: Cancel outdated jobs
+        uses: fkirc/skip-duplicate-actions@v5
+        with:
+          cancel_others: 'true'
       - name: Set up ROS ecosystem
         uses: ros-tooling/setup-ros@v0.7
 
@@ -45,5 +49,5 @@ jobs:
           . /opt/ros/iron/setup.sh
           . install/setup.sh
           # Run tests for all packages
-          colcon test --packages-skip-regex pylon --event-handlers console_direct+ --return-code-on-test-failure --parallel-workers 1
+          colcon test --event-handlers console_direct+ --return-code-on-test-failure --parallel-workers 1
         working-directory: /colcon_ws

.vscode/c_cpp_properties.json

@@ -16,7 +16,8 @@
                 "/root/colcon_ws/build/**/rosidl_generator_cpp/**",
                 "${workspaceFolder}/**/include/**",
                 "/opt/ros/${env:ROS_DISTRO}/include/**",
-                "/usr/include/**"
+                "/usr/include/**",
+                "/opt/pylon/include/**"
             ],
             "name": "ROS",
             "intelliSenseMode": "gcc-x64",

.vscode/extensions.json

@@ -1,27 +1,28 @@
 {
     "recommendations": [
-        "cschlosser.doxdocgen",  // Generates doxygen comments
-        "DavidAnson.vscode-markdownlint",  // Lints markdown files
-        "DotJoshJohnson.xml",  // XML support
-        "eamodio.gitlens",  // Extended git highlighting
-        "lextudio.restructuredtext",  // Rendering rst files in preview
-        "mohsen1.prettify-json",  // JSON formatting
-        "ms-azuretools.vscode-docker",  // Docker support
-        "MS-CEINTL.vscode-language-pack-de",  // German language pack
-        "ms-iot.vscode-ros",  // ROS support
-        "ms-python.python",  // Python support
-        "ms-python.vscode-pylance",  // Python support
-        "ms-vscode.cmake-tools",  // CMake support
-        "ms-vscode.cpptools-extension-pack",  // C++ support
-        "ms-vscode.cpptools-themes",  // C++ support
-        "ms-vscode.cpptools",  // C++ support
-        "njpwerner.autodocstring",  // Generates docstrings
-        "streetsidesoftware.code-spell-checker-german",  // Spell checker
-        "streetsidesoftware.code-spell-checker",  // Spell checker
-        "tamasfe.even-better-toml",  // TOML support
-        "trond-snekvik.simple-rst",  // Syntax highlighting for rst files
-        "twxs.cmake",  // CMake support
-        "zxh404.vscode-proto3",  // Protobuf support
+        "charliermarsh.ruff", // Ruff linting and formatting support
+        "cschlosser.doxdocgen", // Generates doxygen comments
+        "DavidAnson.vscode-markdownlint", // Lints markdown files
+        "DotJoshJohnson.xml", // XML support
+        "eamodio.gitlens", // Extended git highlighting
+        "lextudio.restructuredtext", // Rendering rst files in preview
         "Mastermori.dsd", // DSD support
+        "mohsen1.prettify-json", // JSON formatting
+        "ms-azuretools.vscode-docker", // Docker support
+        "MS-CEINTL.vscode-language-pack-de", // German language pack
+        "ms-iot.vscode-ros", // ROS support
+        "ms-python.python", // Python support
+        "ms-python.vscode-pylance", // Python support
+        "ms-vscode.cmake-tools", // CMake support
+        "ms-vscode.cpptools-extension-pack", // C++ support
+        "ms-vscode.cpptools-themes", // C++ support
+        "ms-vscode.cpptools", // C++ support
+        "njpwerner.autodocstring", // Generates docstrings
+        "streetsidesoftware.code-spell-checker-german", // Spell checker
+        "streetsidesoftware.code-spell-checker", // Spell checker
+        "tamasfe.even-better-toml", // TOML support
+        "trond-snekvik.simple-rst", // Syntax highlighting for rst files
+        "twxs.cmake", // CMake support
+        "zxh404.vscode-proto3", // Protobuf support
     ]
 }

.vscode/settings.json

@@ -190,9 +190,10 @@
         "typeindex": "cpp",
         "typeinfo": "cpp",
         "valarray": "cpp",
-        "variant": "cpp"
+        "variant": "cpp",
+        "regex": "cpp",
+        "future": "cpp"
     },
-
     // Tell the ROS extension where to find the setup.bash
     // This also utilizes the COLCON_WS environment variable, which needs to be set
     "ros.distro": "iron",
@@ -204,4 +205,6 @@
         "/opt/ros/iron/lib/python3.10/site-packages"
     ],
     "cmake.configureOnOpen": false,
+    "editor.formatOnSave": true,
+    "editor.defaultFormatter": "charliermarsh.ruff",
 }

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py

@@ -89,6 +89,9 @@ def stop_walk(self):
         # send special command to walking to stop it
         msg = Twist()
         msg.angular.x = -1.0
+        # Cancel the path planning
+        self.cancel_goal()
+        # Publish the stop command
         self.direct_cmd_vel_pub.publish(msg)
 
     def calculate_time_to_ball(self):

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py

@@ -69,11 +69,15 @@ 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", "penalty_area_length", "goal_area_length"],
         )
         self.field_length: float = parameters["field_length"]
         self.field_width: float = parameters["field_width"]
         self.goal_width: float = parameters["goal_width"]
+        self.penalty_area_length: float = parameters["penalty_area_length"]
+        self.goal_area_length: float = parameters["goal_area_length"]
         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"
@@ -169,6 +173,9 @@ def get_best_ball_point_stamped(self) -> PointStamped:
             return self.ball_odom
 
     def get_ball_position_uv(self) -> Tuple[float, float]:
+        """
+        Returns the ball position relative to the robot in the base_footprint frame
+        """
         ball = self.get_best_ball_point_stamped()
         try:
             ball_bfp = self._blackboard.tf_buffer.transform(

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/actions/dribble_forward.py

@@ -10,15 +10,8 @@ class DribbleForward(AbstractActionElement):
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
         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.current_speed_x = self.blackboard.pathfinding.current_cmd_vel.linear.x
-        self.current_speed_y = self.blackboard.pathfinding.current_cmd_vel.linear.y
 
     def perform(self, reevaluate=False):
         """
@@ -29,25 +22,15 @@ def perform(self, reevaluate=False):
         """
         # Get the ball relative to the base fottprint
         _, ball_v = self.blackboard.world_model.get_ball_position_uv()
-        # Get the relative angle from us to the ball
-        ball_angle = self.blackboard.world_model.get_ball_angle()
-
-        # 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)
-        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)
 
-        # give more speed in y direction based on ball position
-        y_speed = ball_v * self.p
+        self.current_speed_y = np.clip(ball_v * self.p, -self.max_speed_y, self.max_speed_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)
+        # Stop the pathfinding if it is running for some reason
+        self.blackboard.pathfinding.cancel_goal()
 
+        # Publish the velocities
         cmd_vel = Twist()
-        cmd_vel.linear.x = self.current_speed_x
+        cmd_vel.linear.x = self.max_speed_x
         cmd_vel.linear.y = self.current_speed_y
         cmd_vel.angular.z = 0.0
         self.blackboard.pathfinding.direct_cmd_vel_pub.publish(cmd_vel)

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/actions/go_to_block_position.py

@@ -1,4 +1,8 @@
+import math
+
+import numpy as np
 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
 
@@ -9,7 +13,15 @@ class GoToBlockPosition(AbstractActionElement):
     def __init__(self, blackboard, dsd, parameters):
         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"]
+        self.block_radius = self.blackboard.config["block_radius_robot"]
+        self.left_goalpost_position = [
+            -self.blackboard.world_model.field_length / 2,
+            self.blackboard.world_model.goal_width / 2,
+        ]  # position of left goalpost
+        self.right_goalpost_position = [
+            -self.blackboard.world_model.field_length / 2,
+            -self.blackboard.world_model.goal_width / 2,
+        ]  # position of right goalpost
 
     def perform(self, reevaluate=False):
         # The block position should be a position between the ball and the center of the goal
@@ -27,28 +39,132 @@ def perform(self, reevaluate=False):
         #      |
         #      +------------------+--------------> x
         #                         0
-
-        goal_position = (-self.blackboard.world_model.field_length / 2, 0)  # position of the own goal
         ball_position = self.blackboard.world_model.get_ball_position_xy()
 
-        x_delta = ball_position[0] - goal_position[0]
-        y_delta = ball_position[1]  # goal Y-position is always 0
-        gradient = float(y_delta) / float(x_delta) * self.block_position_gradient_factor
-        goalie_y = self.block_position_goal_offset * gradient
+        ball_to_line_distance = ball_position[0] - self.left_goalpost_position[0]
+
+        opening_angle = self._calc_opening_angle(
+            ball_to_line_distance, ball_position
+        )  # angle of ball to both goalposts
+        angle_bisector = opening_angle / 2  # of the angle between the goalpost
+
+        goalie_pos = self._get_robot_pose(angle_bisector, ball_to_line_distance, ball_position)
+
+        goalie_pos = self._cut_goalie_pos(goalie_pos, ball_position)
 
         pose_msg = PoseStamped()
         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.y = float(self._stay_in_front_of_goal(goalie_y))
-        pose_msg.pose.orientation.w = 1.0
+        pose_msg.pose.position.x = float(goalie_pos[0])
+        pose_msg.pose.position.y = float(goalie_pos[1])
+
+        yaw = self._calc_turn_to_ball_angle(ball_position, goalie_pos)
+        pose_msg.pose.orientation = quat_from_yaw(yaw)
 
         self.blackboard.pathfinding.publish(pose_msg)
 
-    def _stay_in_front_of_goal(self, y):
-        # keeps the y-values of the position in between of the goalposts.
-        # this ensures, that y is in [-self.blackboard.world_model.goal_width / 2, self.blackboard.world_model.goal_width / 2].
-        return max(-self.blackboard.world_model.goal_width / 2, min(self.blackboard.world_model.goal_width / 2, y))
+    def _calc_opening_angle(self, ball_to_line: float, ball_pos: tuple) -> float:
+        """
+        Calculates the opening angle of the ball to both goalposts.
+        With it we can get the angle bisector, in which we place the robot.
+        Args:
+            ball_to_line: distance of the ball to our goal line
+            ball_pos: ball position in world koordinate system
+
+        Returns:
+            float: opening angle
+        """
+        ball_angle_left = np.arctan2(
+            ball_pos[1] - self.left_goalpost_position[1], ball_to_line
+        )  # angle of ball to left goalpost
+        ball_angle_right = np.arctan2(
+            ball_pos[1] - self.right_goalpost_position[1], ball_to_line
+        )  # angle of ball to right goalpost
+        opening_angle_ball = ball_angle_right - ball_angle_left  # Subtract to get the opening angle
+        return opening_angle_ball
+
+    def _get_robot_pose(
+        self, angle_bisector: float, ball_to_line_distance: float, ball_pos: tuple[float, float]
+    ) -> tuple[float, float]:
+        """
+        Calculates the position where the robot should be to block the ball.
+        The position is the place where the circle of the robot blocking radius is touching both lines
+        from the ball to the goalposts. So the goal is completely guarded.
+
+        Args:
+            angle_bisector: bisector angle of the ball with goalposts
+            ball_to_line_distance: distance of the ball to the goal line
+            ball_pos: ball position in world koordinate system
+
+        Returns:
+            tuple: goalie_position
+        """
+        left_goalpost_to_ball = math.dist(self.left_goalpost_position, ball_pos)
+        right_goalpost_to_ball = math.dist(self.right_goalpost_position, ball_pos)
+
+        wanted_distance_robot_to_ball = self.block_radius / np.sin(angle_bisector)  # ball obstruction distance of robot
+        angle_of_left_goalpost_to_ball = np.arccos(ball_to_line_distance / left_goalpost_to_ball)
+        angle_of_right_goalpost_to_ball = np.arccos(ball_to_line_distance / right_goalpost_to_ball)
+
+        if ball_pos[1] > self.blackboard.world_model.goal_width / 2:
+            angle_to_bisector = angle_of_left_goalpost_to_ball + angle_bisector  # here left goalpost angle used
+            goalie_x = ball_pos[0] - wanted_distance_robot_to_ball * np.cos(angle_to_bisector)
+            goalie_y = ball_pos[1] - wanted_distance_robot_to_ball * np.sin(angle_to_bisector)
+        elif ball_pos[1] < -self.blackboard.world_model.goal_width / 2:
+            angle_to_bisector = angle_of_right_goalpost_to_ball + angle_bisector  # here right goalpost angle used
+            goalie_x = ball_pos[0] - wanted_distance_robot_to_ball * np.cos(angle_to_bisector)
+            goalie_y = ball_pos[1] + wanted_distance_robot_to_ball * np.sin(angle_to_bisector)
+        else:
+            angle_to_bisector = angle_of_left_goalpost_to_ball - angle_bisector  # here right goalpost angle used
+            goalie_x = ball_pos[0] - wanted_distance_robot_to_ball * np.cos(angle_to_bisector)
+            goalie_y = ball_pos[1] + wanted_distance_robot_to_ball * np.sin(angle_to_bisector)
+        # calculate the angle, the robot needs to turn to look at the ball
+        return (goalie_x, goalie_y)
+
+    def _cut_goalie_pos(self, goalie_pos: tuple[float, float], ball_pos: tuple[float, float]) -> tuple[float, float]:
+        """
+        Cut the goalie position if he is behind the goal or wants to leave the designated
+        goal area.
+
+        Args:
+            goalie_pos: a tuple which contains [goalie_y, goalie_x] position
+            ball_pos: a tuple which contains [ball_y, ball_x] position
+
+        Returns:
+            tuple: goalie_position
+        """
+        goalie_pos_x, goalie_pos_y = goalie_pos
+        # cut if goalie is behind the goal happens when the ball is too close to the corner
+        if goalie_pos_x < -self.blackboard.world_model.field_length / 2:
+            goalie_pos_x = -self.blackboard.world_model.field_length / 2 + self.block_position_goal_offset
+            # instead put goalie to goalpost position
+            if ball_pos[1] > 0:
+                goalie_pos_y = self.blackboard.world_model.goal_width / 2
+            else:
+                goalie_pos_y = -self.blackboard.world_model.goal_width / 2
+        # cut so goalie does not leave goalkeeper area
+        goalie_pos_x = np.clip(
+            goalie_pos_x,
+            -self.blackboard.world_model.field_length / 2,
+            -self.blackboard.world_model.field_length / 2
+            + self.blackboard.world_model.penalty_area_length
+            - self.block_radius,
+        )
+        return (goalie_pos_x, goalie_pos_y)
+
+    def _calc_turn_to_ball_angle(self, ball_pos: tuple[float, float], goalie_pos: tuple[float, float]) -> float:
+        """
+        Calculates the angle the robot needs to turn to look at the ball.
+
+        Args:
+            ball_pos: a tuple which contains [ball_y, ball_x] position
+            goalie_pos: a tuple which contains [goalie_y, goalie_x] position
+
+        Returns:
+            float: angle
+        """
+        robot_to_ball_x = ball_pos[0] - goalie_pos[0]
+        robot_to_ball_y = ball_pos[1] - goalie_pos[1]
+        angle = np.arctan2(robot_to_ball_y, robot_to_ball_x)
+        return angle

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/actions/kick_ball.py

@@ -27,10 +27,16 @@ def __init__(self, blackboard, dsd, parameters):
             self.blackboard.node.get_logger().error(
                 "The parameter '{}' could not be used to decide which foot should kick".format(parameters["foot"])
             )
+        self.requested = False
 
     def perform(self, reevaluate=False):
-        if not self.blackboard.animation.is_busy():
+        if not self.requested:
+            # Request play animation
             self.blackboard.animation.play_animation(self.kick, False)
+            self.requested = True
+
+        if not self.blackboard.animation.is_busy() and self.requested:
+            self.pop()
 
 
 class KickBallDynamic(AbstractKickAction):
@@ -51,8 +57,6 @@ def __init__(self, blackboard, dsd, parameters):
         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)
 
     def perform(self, reevaluate=False):
         self.publish_debug_data("Currently Kicking", self.blackboard.kick.is_currently_kicking)