fix(whistle_detection): correctly localize during STATE_SET + whistle

which happends, when switching from `STATE_SET` to `STATE_PLAYING` by
whistle, but the GameController only delivers the `STATE_PLAYING` after
the whistle detection "cooldown".

Author: cleWu03

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/game_status_capsule.py

@@ -2,6 +2,7 @@
 
 from bitbots_utils.utils import get_parameters_from_other_node
 from game_controller_hsl_interfaces.msg import GameState
+from rclpy.time import Time
 from std_msgs.msg import Bool
 
 from bitbots_blackboard.capsules import AbstractBlackboardCapsule
@@ -20,7 +21,7 @@ def __init__(self, node, blackboard=None):
         self.last_goal_from_us_time = -86400.0
         self.last_goal_time = -86400.0
         self.free_kick_kickoff_team: Optional[bool] = None
-        self.whistle_detected: bool = False
+        self.last_timestep_whistle_detected: Time = self._node.get_clock().now()
         self.game_controller_stop: bool = False
         # publish stopped msg for hcm
         self.stop_pub = node.create_publisher(Bool, "game_controller/stop_msg", 1)
@@ -129,5 +130,5 @@ def gamestate_callback(self, gamestate_msg: GameState) -> None:
         self.last_update = self._node.get_clock().now().nanoseconds / 1e9
         self.gamestate = gamestate_msg
 
-    def whistle_detection_callback(self, msg) -> None:
-        self.whistle_detected = msg.data
+    def whistle_detection_callback(self, _) -> None:
+        self.last_timestep_whistle_detected = self._node.get_clock().now()

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/whistle_detected.py

@@ -7,13 +7,13 @@ class WhistleDetected(AbstractDecisionElement):
 
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
-        self.bb = blackboard
-        self.bb.gamestate.whistle_detected = False
+        self.blackboard = blackboard
+        self.last_timestep_whistle_detected = self.blackboard.gamestate.last_timestep_whistle_detected
 
     def perform(self, reevaluate=False):
-        if self.bb.gamestate.whistle_detected:
-            return "YES"
-        return "NO"
+        if self.last_timestep_whistle_detected == self.blackboard.gamestate.last_timestep_whistle_detected:
+            return "NOT_DETECTED"
+        return "DETECTED"
 
     def get_reevaluate(self):
         return True

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd

@@ -154,8 +154,8 @@ $IsPenalized
                 NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2
                 DONE --> #PositioningReady
         SET --> $WhistleDetected
-            YES --> #PlayingBehavior
-            NO --> $SecondaryStateDecider
+            DETECTED --> #PlayingBehavior
+            NOT_DETECTED --> $SecondaryStateDecider
                 PENALTYSHOOT --> $SecondaryStateTeamDecider
                     OUR --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @PlayAnimationInitInSim + r:false, @GetWalkready + r:false, @Stand // we need to also see the goalie
                     OTHER --> $BallSeen

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py

@@ -11,7 +11,7 @@
 from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from soccer_vision_3d_msgs.msg import RobotArray
-from std_msgs.msg import Bool
+from std_msgs.msg import Empty
 
 from bitbots_body_behavior import behavior_dsd
 from bitbots_msgs.msg import RobotControlState, TeamData
@@ -77,7 +77,7 @@ def __init__(self, node: Node):
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
         node.create_subscription(
-            Bool,
+            Empty,
             "whistle_detected",
             blackboard.gamestate.whistle_detection_callback,
             qos_profile=1,

src/bitbots_misc/bitbots_whistle_detector/bitbots_whistle_detector/whistle_detector.py

@@ -6,15 +6,15 @@
 from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from rclpy.qos import qos_profile_sensor_data
-from std_msgs.msg import Bool
+from std_msgs.msg import Empty
 
 
 class WhistleDetector(Node):
     def __init__(self) -> None:
         super().__init__("whistle_detector")
         self.logger = self.get_logger()
 
-        self.whistle_publisher = self.create_publisher(Bool, "whistle_detected", 1)
+        self.whistle_publisher = self.create_publisher(Empty, "whistle_detected", 1)
 
         self.audio_buffer = np.array([], dtype=np.float32)
         self.sample_rate = 16000
@@ -45,7 +45,7 @@ def process_audio(self) -> None:
         whistle_detected = self.detect_whistle(audio, self.sample_rate)
 
         if whistle_detected:
-            msg = Bool(data=True)
+            msg = Empty()
             self.whistle_publisher.publish(msg)
 
     def detect_whistle(self, audio, sample_rate):

src/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/decisions/whistle_detected.py

@@ -0,0 +1,25 @@
+from bitbots_localization_handler.localization_dsd.decisions import AbstractLocalizationDecisionElement
+from bitbots_localization_handler.localization_dsd.localization_blackboard import LocalizationBlackboard
+
+
+class WhistleDetected(AbstractLocalizationDecisionElement):
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.blackboard: LocalizationBlackboard
+        self.last_timestep_whistle_detected = self.blackboard.last_timestep_whistle_detected
+
+    def perform(self, reevaluate=False):
+        """
+        Checks if a whistle was detected
+        """
+
+        if self.last_timestep_whistle_detected == self.blackboard.last_timestep_whistle_detected:
+            return "NOT_DETECTED"
+        else:
+            return "DETECTED"
+
+    def get_reevaluate(self):
+        """
+        Game state can change during the game
+        """
+        return True

src/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization.dsd

@@ -22,5 +22,7 @@ $GettingUpState
             NO --> $InitialToReady
                 YES --> @InitSide, @DoNothing
                 NO --> $GameStateDecider
-                    SET --> #InitSet
+                    SET --> $WhistleDetected
+                        NOT_DETECTED --> #InitSet
+                        DETECTED --> @DoNothing
                     ELSE --> @DoNothing

src/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_dsd/localization_blackboard.py

@@ -10,6 +10,7 @@
 from jaxtyping import Float
 from rclpy.duration import Duration
 from rclpy.node import Node
+from rclpy.time import Time
 from ros2_numpy import numpify
 from sensor_msgs.msg import Imu
 from tf2_geometry_msgs import TransformStamped
@@ -69,6 +70,9 @@ def __init__(self, node: Node):
         self.last_init_action_type: Optional[type] = None
         self.last_init_odom_transform: TransformStamped | None = None
 
+        # Last time we have detected a whistle
+        self.last_timestep_whistle_detected: Time = self.node.get_clock().now()
+
     def _callback_pose(self, msg: PoseWithCovarianceStamped):
         self.robot_pose = msg
 
@@ -122,3 +126,6 @@ def get_localization_yaw(self) -> float:
         if self.robot_pose is None:
             return 0.0
         return quat2euler(xyzw2wxyz(numpify(self.robot_pose.pose.pose.orientation)), axes="szxy")[0]
+
+    def whistle_detection_callback(self, _):
+        self.last_timestep_whistle_detected = self.node.get_clock().now()

src/bitbots_navigation/bitbots_localization_handler/bitbots_localization_handler/localization_handler.py

@@ -9,6 +9,7 @@
 from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from sensor_msgs.msg import Imu
+from std_msgs.msg import Empty
 
 from bitbots_localization_handler import localization_dsd
 from bitbots_localization_handler.localization_dsd.localization_blackboard import LocalizationBlackboard
@@ -48,6 +49,13 @@ def init(node: Node):
     node.create_subscription(
         Imu, "/imu/data", blackboard._callback_imu, 1, callback_group=MutuallyExclusiveCallbackGroup()
     )
+    node.create_subscription(
+        Empty,
+        "whistle_detected",
+        blackboard.whistle_detection_callback,
+        qos_profile=1,
+        callback_group=MutuallyExclusiveCallbackGroup(),
+    )
 
     return dsd