Merge branch 'main' into feature/projekt24/pathfinding

.github/workflows/pre-commit.yml

@@ -7,7 +7,7 @@ on:
 
 jobs:
   pre-commit:
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v4
       - uses: actions/setup-python@v4

bitbots_behavior/bitbots_blackboard/CMakeLists.txt

@@ -1,81 +1,12 @@
 cmake_minimum_required(VERSION 3.5)
 project(bitbots_blackboard)
 
-# Add support for C++17
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-
-find_package(ament_cmake REQUIRED)
-find_package(bio_ik_msgs REQUIRED)
 find_package(bitbots_docs REQUIRED)
-find_package(bitbots_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(rclpy REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(std_srvs REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-
-set(INCLUDE_DIRS
-    ${bio_ik_msgs_INCLUDE_DIRS}
-    ${ament_cmake_INCLUDE_DIRS}
-    ${sensor_msgs_INCLUDE_DIRS}
-    ${rclpy_INCLUDE_DIRS}
-    ${tf2_INCLUDE_DIRS}
-    ${bitbots_msgs_INCLUDE_DIRS}
-    ${std_msgs_INCLUDE_DIRS}
-    ${tf2_geometry_msgs_INCLUDE_DIRS}
-    ${std_srvs_INCLUDE_DIRS}
-    ${geometry_msgs_INCLUDE_DIRS}
-    ${bitbots_docs_INCLUDE_DIRS})
-include_directories(${INCLUDE_DIRS})
-
-set(LIBRARY_DIRS
-    ${bio_ik_msgs_LIBRARY_DIRS}
-    ${ament_cmake_LIBRARY_DIRS}
-    ${sensor_msgs_LIBRARY_DIRS}
-    ${rclpy_LIBRARY_DIRS}
-    ${tf2_LIBRARY_DIRS}
-    ${bitbots_msgs_LIBRARY_DIRS}
-    ${std_msgs_LIBRARY_DIRS}
-    ${tf2_geometry_msgs_LIBRARY_DIRS}
-    ${std_srvs_LIBRARY_DIRS}
-    ${geometry_msgs_LIBRARY_DIRS}
-    ${bitbots_docs_LIBRARY_DIRS})
-
-link_directories(${LIBRARY_DIRS})
-
-set(LIBS
-    ${bio_ik_msgs_LIBRARIES}
-    ${ament_cmake_LIBRARIES}
-    ${sensor_msgs_LIBRARIES}
-    ${rclpy_LIBRARIES}
-    ${tf2_LIBRARIES}
-    ${bitbots_msgs_LIBRARIES}
-    ${std_msgs_LIBRARIES}
-    ${tf2_geometry_msgs_LIBRARIES}
-    ${std_srvs_LIBRARIES}
-    ${geometry_msgs_LIBRARIES}
-    ${bitbots_docs_LIBRARIES})
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
 
-include(${CMAKE_BINARY_DIR}/../bitbots_docs/enable_bitbots_docs.cmake)
 enable_bitbots_docs()
 
-ament_export_dependencies(bio_ik_msgs)
-ament_export_dependencies(ament_cmake)
-ament_export_dependencies(sensor_msgs)
-ament_export_dependencies(rclpy)
-ament_export_dependencies(tf2)
-ament_export_dependencies(bitbots_msgs)
-ament_export_dependencies(std_msgs)
-ament_export_dependencies(tf2_geometry_msgs)
-ament_export_dependencies(std_srvs)
-ament_export_dependencies(geometry_msgs)
-ament_export_dependencies(bitbots_docs)
-ament_export_include_directories(${INCLUDE_DIRS})
-
 if(BUILD_TESTING)
   find_package(ament_cmake_mypy REQUIRED)
   ament_mypy(CONFIG_FILE "${CMAKE_CURRENT_LIST_DIR}/mypy.ini")

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py

@@ -1,5 +1,5 @@
 import math
-from typing import List, Optional, Tuple
+from typing import Optional
 
 import numpy as np
 import tf2_ros as tf2
@@ -132,7 +132,7 @@ def get_pass_regions(self) -> np.ndarray:
         # Smooth obstacle map
         return gaussian_filter(costmap, pass_smooth)
 
-    def field_2_costmap_coord(self, x: float, y: float) -> Tuple[int, int]:
+    def field_2_costmap_coord(self, x: float, y: float) -> tuple[int, int]:
         """
         Converts a field position to the corresponding indices for the costmap.
 
@@ -209,7 +209,7 @@ def calc_base_costmap(self) -> None:
             0 : self.field_width + self.map_margin * 2 : (self.field_width + self.map_margin * 2) * 10j,  # type: ignore[misc]
         ]
 
-        fix_points: List[Tuple[Tuple[float, float], float]] = []
+        fix_points: list[tuple[tuple[float, float], float]] = []
 
         # Add base points
         fix_points.extend(
@@ -284,7 +284,7 @@ def calc_base_costmap(self) -> None:
         self.base_costmap = base_costmap
         self.costmap = self.base_costmap.copy()
 
-    def get_gradient_at_field_position(self, x: float, y: float) -> Tuple[float, float]:
+    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 coordinate in the x direction

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/misc_capsule.py

@@ -8,12 +8,12 @@
 from bitbots_msgs.msg import Audio, HeadMode, RobotControlState
 
 THeadMode: TypeAlias = Literal[  # type: ignore[valid-type]
-    HeadMode.BALL_MODE,
-    HeadMode.FIELD_FEATURES,
+    HeadMode.SEARCH_BALL,
+    HeadMode.SEARCH_FIELD_FEATURES,
     HeadMode.LOOK_FORWARD,
     HeadMode.DONT_MOVE,
-    HeadMode.BALL_MODE_PENALTY,
-    HeadMode.LOOK_FRONT,
+    HeadMode.SEARCH_BALL_PENALTY,
+    HeadMode.SEARCH_FRONT,
 ]
 
 

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/team_data_capsule.py

@@ -1,4 +1,4 @@
-from typing import List, Literal, Optional, Tuple
+from typing import Literal, Optional
 
 import numpy as np
 from bitbots_utils.utils import get_parameters_from_other_node
@@ -142,7 +142,7 @@ def set_action(self, action: int) -> None:
         self.strategy.action = action
         self.action_update = self._node.get_clock().now().nanoseconds / 1e9
 
-    def get_action(self) -> Tuple[int, float]:
+    def get_action(self) -> tuple[int, float]:
         return self.strategy.action, self.action_update
 
     def set_role(self, role: Literal["goalie", "offense", "defense"]) -> None:
@@ -154,7 +154,7 @@ def set_role(self, role: Literal["goalie", "offense", "defense"]) -> None:
         self.strategy.role = self.roles_mapping[role]
         self.role_update = self._node.get_clock().now().nanoseconds / 1e9
 
-    def get_role(self) -> Tuple[int, float]:
+    def get_role(self) -> tuple[int, float]:
         return self.strategy.role, self.role_update
 
     def set_kickoff_strategy(
@@ -164,10 +164,10 @@ def set_kickoff_strategy(
         self.strategy.offensive_side = strategy
         self.strategy_update = self._node.get_clock().now().nanoseconds / 1e9
 
-    def get_kickoff_strategy(self) -> Tuple[int, float]:
+    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]:
+    def get_active_teammate_poses(self, count_goalies: bool = False) -> list[Pose]:
         """Returns the poses of all playing robots"""
         poses = []
         data: TeamData

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py

@@ -1,5 +1,4 @@
 import math
-from typing import Tuple
 
 import numpy as np
 import tf2_ros as tf2
@@ -90,7 +89,7 @@ def ball_has_been_seen(self) -> bool:
         """Returns true if we or a teammate are reasonably sure that we have seen the ball"""
         return self.ball_seen_self() or self._blackboard.team_data.teammate_ball_is_valid()
 
-    def get_ball_position_xy(self) -> Tuple[float, float]:
+    def get_ball_position_xy(self) -> tuple[float, float]:
         """Return the ball saved in the map frame, meaning the absolute position of the ball on the field"""
         ball = self.get_best_ball_point_stamped()
         return ball.point.x, ball.point.y
@@ -117,7 +116,7 @@ def get_best_ball_point_stamped(self) -> PointStamped:
         self.debug_publisher_which_ball.publish(Header(stamp=own_ball.header.stamp, frame_id="own_ball_map"))
         return own_ball
 
-    def get_ball_position_uv(self) -> Tuple[float, float]:
+    def get_ball_position_uv(self) -> tuple[float, float]:
         """
         Returns the ball position relative to the robot in the base_footprint frame.
         U and V are returned, where positive U is forward, positive V is to the left.
@@ -183,18 +182,18 @@ def forget_ball(self) -> None:
     # Goal #
     ########
 
-    def get_map_based_opp_goal_center_uv(self) -> Tuple[float, float]:
+    def get_map_based_opp_goal_center_uv(self) -> tuple[float, float]:
         x, y = self.get_map_based_opp_goal_center_xy()
         return self.get_uv_from_xy(x, y)
 
-    def get_map_based_opp_goal_center_xy(self) -> Tuple[float, float]:
+    def get_map_based_opp_goal_center_xy(self) -> tuple[float, float]:
         return self.field_length / 2, 0.0
 
-    def get_map_based_own_goal_center_uv(self) -> Tuple[float, float]:
+    def get_map_based_own_goal_center_uv(self) -> tuple[float, float]:
         x, y = self.get_map_based_own_goal_center_xy()
         return self.get_uv_from_xy(x, y)
 
-    def get_map_based_own_goal_center_xy(self) -> Tuple[float, float]:
+    def get_map_based_own_goal_center_xy(self) -> tuple[float, float]:
         return -self.field_length / 2, 0.0
 
     def get_map_based_opp_goal_angle_from_ball(self) -> float:
@@ -210,19 +209,19 @@ def get_map_based_opp_goal_angle(self) -> float:
         x, y = self.get_map_based_opp_goal_center_uv()
         return math.atan2(y, x)
 
-    def get_map_based_opp_goal_left_post_uv(self) -> Tuple[float, float]:
+    def get_map_based_opp_goal_left_post_uv(self) -> tuple[float, float]:
         x, y = self.get_map_based_opp_goal_center_xy()
         return self.get_uv_from_xy(x, y - self.goal_width / 2)
 
-    def get_map_based_opp_goal_right_post_uv(self) -> Tuple[float, float]:
+    def get_map_based_opp_goal_right_post_uv(self) -> tuple[float, float]:
         x, y = self.get_map_based_opp_goal_center_xy()
         return self.get_uv_from_xy(x, y + self.goal_width / 2)
 
     ########
     # Pose #
     ########
 
-    def get_current_position(self) -> Tuple[float, float, float]:
+    def get_current_position(self) -> tuple[float, float, float]:
         """
         Returns the current position on the field as determined by the localization.
         0,0,0 is the center of the field looking in the direction of the opponent goal.
@@ -261,7 +260,7 @@ def get_current_position_transform(self) -> TransformStamped:
     # Common #
     ##########
 
-    def get_uv_from_xy(self, x: float, y: float) -> Tuple[float, float]:
+    def get_uv_from_xy(self, x: float, y: float) -> tuple[float, float]:
         """Returns the relativ positions of the robot to this absolute position"""
         current_position = self.get_current_position()
         x2 = x - current_position[0]
@@ -271,7 +270,7 @@ def get_uv_from_xy(self, x: float, y: float) -> Tuple[float, float]:
         v = math.cos(theta) * y2 - math.sin(theta) * x2
         return u, v
 
-    def get_xy_from_uv(self, u: float, v: float) -> Tuple[float, float]:
+    def get_xy_from_uv(self, u: float, v: float) -> tuple[float, float]:
         """Returns the absolute position from the given relative position to the robot"""
         pos_x, pos_y, theta = self.get_current_position()
         angle = math.atan2(v, u) + theta

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/head_modes.py

@@ -45,15 +45,15 @@ class SearchBall(AbstractHeadModeElement):
     """Look for ball"""
 
     def perform(self):
-        self.blackboard.misc.set_head_duty(HeadMode.BALL_MODE)
+        self.blackboard.misc.set_head_duty(HeadMode.SEARCH_BALL)
         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)
+        self.blackboard.misc.set_head_duty(HeadMode.SEARCH_FIELD_FEATURES)
         return self.pop()
 
 
@@ -77,13 +77,13 @@ class LookAtBallPenalty(AbstractHeadModeElement):
     """Ball Mode adapted for Penalty Kick"""
 
     def perform(self):
-        self.blackboard.misc.set_head_duty(HeadMode.BALL_MODE_PENALTY)
+        self.blackboard.misc.set_head_duty(HeadMode.SEARCH_BALL_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)
+        self.blackboard.misc.set_head_duty(HeadMode.SEARCH_FRONT)
         return self.pop()

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/ball_kick_area.py

@@ -25,6 +25,9 @@ def perform(self, reevaluate=False):
         ball_position = self.blackboard.world_model.get_ball_position_uv()
 
         self.publish_debug_data("ball_position", {"u": ball_position[0], "v": ball_position[1]})
+        self.publish_debug_data(
+            "smoothing_close", f"{self.no_near_decisions} ({self.no_near_decisions / self.smoothing * 10}%)"
+        )
 
         # Check if the ball is in the enter area
         if 0 <= ball_position[0] <= self.kick_x_enter and 0 <= abs(ball_position[1]) <= self.kick_y_enter:

bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -146,7 +146,7 @@ body_behavior:
     dribble_ball_distance_threshold: 0.5
     dribble_kick_angle: 0.6
 
-    kick_decision_smoothing: 5.0
+    kick_decision_smoothing: 1.0
 
     ##################
     # costmap params #

bitbots_lowlevel/bitbots_buttons/CMakeLists.txt

@@ -6,6 +6,11 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
+# Build with release optimizations and debug symbols by default
+if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE RelWithDebug)
+endif()
+
 find_package(ament_cmake REQUIRED)
 find_package(backward_ros REQUIRED)
 find_package(bitbots_docs REQUIRED)

bitbots_lowlevel/bitbots_ros_control/CMakeLists.txt

@@ -6,6 +6,11 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
+# Build with release optimizations and debug symbols by default
+if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE RelWithDebug)
+endif()
+
 find_package(ament_cmake REQUIRED)
 find_package(backward_ros REQUIRED)
 find_package(bitbots_buttons REQUIRED)

bitbots_misc/bitbots_basler_camera/CMakeLists.txt

@@ -6,6 +6,11 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
+# Build with release optimizations and debug symbols by default
+if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE RelWithDebug)
+endif()
+
 find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(backward_ros REQUIRED)

bitbots_misc/bitbots_basler_camera/package.xml

@@ -12,6 +12,7 @@
 
   <author email="info@bit-bots.de">Hamburg Bit-Bots</author>
 
+  <depend>backward_ros</depend>
   <depend>bitbots_docs</depend>
   <depend>camera_info_manager</depend>
   <depend>cv_bridge</depend>

bitbots_misc/bitbots_extrinsic_calibration/CMakeLists.txt

@@ -6,16 +6,21 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 17)
 endif()
 
+# Build with release optimizations and debug symbols by default
+if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE RelWithDebug)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(backward_ros REQUIRED)
+find_package(bitbots_docs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(ament_cmake REQUIRED)
-find_package(tf2_ros REQUIRED)
+find_package(rot_conv REQUIRED)
 find_package(std_msgs REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(bitbots_docs REQUIRED)
 find_package(tf2 REQUIRED)
-find_package(rot_conv REQUIRED)
-find_package(backward_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 add_compile_options(-Wall -Werror -Wno-unused)