Whistle detection (#775)

Author: texhnolyze

pixi.lock

@@ -37,6 +37,7 @@ readline = ">=8.2.999"  # This is needed because we want to install an empty rea
 
 [feature.ros.dependencies]
 # Misc dependencies for our ROS 2 packages
+alsa-plugins = ">=1.2.12,<2"
 beartype = ">=0.22.6,<0.23"
 breathe = ">=4.36.0,<5"
 cmake = "<3.30"  # Constraint to avoid issues with deprecated features in newer cmake versions
@@ -51,6 +52,7 @@ ipython = ">=9.7.0,<10"
 jaxtyping = ">=0.3.2,<0.4"
 jinja2 = ">=3.1.6,<4"
 libboost-devel = ">=1.86.0,<2"
+libflac = "==1.4.3"
 libnuma = ">=2.0.18,<3"
 libopencv = ">=4.11.0,<5"
 libprotobuf = ">=6.31.1,<7"
@@ -62,6 +64,7 @@ opencv = ">=4.11.0,<5"
 paramiko =   ">=4.0.0,<5"
 pkg-config = ">=0.29.2,<0.30"
 playsound = ">=1.3.0,<2"
+portaudio = ">=19.7.0,<20"
 protobuf = ">=6.31.1,<7"
 psutil = ">=7.1.3,<8"
 pthread-stubs = ">=0.4,<0.5"
@@ -84,6 +87,7 @@ colcon-common-extensions = ">=0.3.0,<0.4"
 colcon-notification = ">=0.3.0,<0.4"
 ros-jazzy-action-msgs = ">=2.0.3,<3"
 ros-jazzy-ament-cmake = ">=2.5.4,<3"
+ros-jazzy-ament-cmake-clang-format = ">=0.17.3,<0.18"
 ros-jazzy-ament-cmake-mypy = ">=0.17.3,<0.18"
 ros-jazzy-ament-copyright = ">=0.17.3,<0.18"
 ros-jazzy-ament-flake8 = ">=0.17.3,<0.18"
@@ -131,6 +135,7 @@ ros-jazzy-rosbridge-suite = ">=2.3.0,<3"
 ros-jazzy-rosgraph-msgs = ">=2.0.3,<3"
 ros-jazzy-rosidl-default-generators = ">=1.6.0,<2"
 ros-jazzy-rosidl-default-runtime = ">=1.6.0,<2"
+ros-jazzy-rosidl-runtime-c = ">=4.6.7,<5"
 ros-jazzy-rqt-gui = ">=1.6.0,<2"
 ros-jazzy-rqt-gui-py = ">=1.6.0,<2"
 ros-jazzy-sensor-msgs = ">=5.3.6,<6"
@@ -165,7 +170,6 @@ ros-jazzy-bio-ik-msgs = ">=0.0.0,<0.0.1"
 ros-jazzy-biped-interfaces = ">=0.0.0,<0.0.1"
 ros-jazzy-bitbots-tf-buffer = ">=1.0.0,<2"
 ros-jazzy-ros2-python-extension = ">=1.0.0,<2"
-ros-jazzy-audio-common = ">=0.3.15,<0.4"
 ros-jazzy-dynamic-stack-decider = ">=0.5.3,<0.6"
 ros-jazzy-dynamic-stack-decider-visualization = ">=0.2.1,<0.3"
 ros-jazzy-game-controller-hl = ">=1.1.0,<2"
@@ -238,6 +242,7 @@ RMW_IMPLEMENTATION = "rmw_cyclonedds_cpp"
 COLCON_LOG_LEVEL = "30"
 PYTHONWARNINGS="ignore:::setuptools.command.install,ignore:::setuptools.command.easy_install,ignore:::pkg_resources,ignore:easy_install command is deprecated,ignore:setup.py install is deprecated"
 WEBOTS_HOME = "$CONDA_PREFIX/share/webots"
+ALSA_PLUGIN_DIR = "/usr/lib/x86_64-linux-gnu/alsa-lib"
 
 [feature.ros.activation]
 scripts = [
@@ -249,3 +254,4 @@ scripts = [
 default = ["ros", "format"]  # Full development environment (excluding robot-only deps)
 format = ["format"]  # Format only environment
 robot = ["ros", "format", "robot"]  # Robot environment with additional robot-only dependencies
+

pixi.toml

@@ -1,7 +1,7 @@
-from typing import Optional
-
 from bitbots_utils.utils import get_parameters_from_other_node
+from builtin_interfaces.msg import Time as TimeMsg
 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
@@ -19,8 +19,9 @@ def __init__(self, node, blackboard=None):
         self.unpenalized_time: float = 0.0
         self.last_goal_from_us_time = -86400.0
         self.last_goal_time = -86400.0
-        self.free_kick_kickoff_team: Optional[bool] = None
+        self.free_kick_kickoff_team: bool | None = None
         self.game_controller_stop: bool = False
+        self.last_timestep_whistle_detected: Time | None = None
         # publish stopped msg for hcm
         self.stop_pub = node.create_publisher(Bool, "game_controller/stop_msg", 1)
 
@@ -127,3 +128,6 @@ 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: TimeMsg) -> None:
+        self.last_timestep_whistle_detected = Time.from_msg(msg)

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

@@ -0,0 +1,24 @@
+from bitbots_blackboard.body_blackboard import BodyBlackboard
+from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement
+
+
+class WhistleDetected(AbstractDecisionElement):
+    blackboard: BodyBlackboard
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.previous_timestep_whistle_detected = self.blackboard.gamestate.last_timestep_whistle_detected
+
+    def perform(self, reevaluate=False):
+        """
+        Returns "DETECTED" if a whistle was detected while the decision element was active, as part of the
+        DSD stack. If we have detected a whistle previously, but not during this decision element being on
+        the decision stack it counts as "NOT_DETECTED".
+        """
+        if self.previous_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/decisions/whistle_detected.py

@@ -130,6 +130,16 @@ $BallSeen
                     SECOND --> #SupporterRole
                     THIRD --> #DefensePositioning
 
+#PlayingBehavior
+$SecondaryStateDecider
+    PENALTYSHOOT --> #PenaltyShootoutBehavior
+    TIMEOUT --> #StandAndLook
+    ELSE --> $SecondaryStateTeamDecider
+        OUR --> #NormalBehavior
+        OTHER --> #Placing
+    NORMAL --> #NormalBehavior
+    OVERTIME --> #NormalBehavior
+
 -->BodyBehavior
 $IsPenalized
     YES --> #DoNothing
@@ -143,23 +153,18 @@ $IsPenalized
             NO --> $DoOnce
                 NOT_DONE --> @ChangeAction + action:waiting + r:false, @LookAtFieldFeatures + r:false, @Stand + duration:2
                 DONE --> #PositioningReady
-        SET --> $SecondaryStateDecider
-            PENALTYSHOOT --> $SecondaryStateTeamDecider
-                OUR --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @PlayAnimationInitInSim + r:false, @GetWalkready + r:false, @Stand
-                OTHER --> $BallSeen
-                    YES --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @PlayAnimationInitInSim + r:false, @GetWalkready + r:false, @TrackBall + r:false, @PlayAnimationGoalieArms + r:false, @Stand // goalie only needs to care about the ball
-                    NO --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @PlayAnimationInitInSim + r:false, @GetWalkready + r:false, @LookAtFieldFeatures + r:false, @PlayAnimationGoalieArms + r:false, @Stand
-            ELSE --> #StandAndLook
+        SET --> $WhistleDetected
+            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
+                        YES --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @PlayAnimationInitInSim + r:false, @GetWalkready + r:false, @TrackBall + r:false, @PlayAnimationGoalieArms + r:false, @Stand // goalie only needs to care about the ball
+                        NO --> @Stand + duration:0.1 + r:false, @DeactivateHCM + r:false, @LookForward + r:false, @PlayAnimationInitInSim + r:false, @GetWalkready + r:false, @LookAtFieldFeatures + r:false, @PlayAnimationGoalieArms + r:false, @Stand
+                ELSE --> #StandAndLook
         FINISHED --> $CurrentScore
             AHEAD --> @Stand + duration:0.5 + r:false, @PlaySound + file:fanfare.wav, @PlayAnimationCheering + r:false, @GetWalkready + r:false, @LookForward, @Stand
             ELSE --> #Init
-        PLAYING --> $SecondaryStateDecider
-            PENALTYSHOOT --> #PenaltyShootoutBehavior
-            TIMEOUT --> #StandAndLook
-            ELSE --> $SecondaryStateTeamDecider
-                OUR --> #NormalBehavior
-                OTHER --> #Placing
-            NORMAL --> #NormalBehavior
-            OVERTIME --> #NormalBehavior
+        PLAYING --> #PlayingBehavior
         STANDBY --> #StandAndLook
         STOPPED --> #DoNothing

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

@@ -3,6 +3,7 @@
 import rclpy
 from bitbots_blackboard.body_blackboard import BodyBlackboard
 from bitbots_tf_buffer import Buffer
+from builtin_interfaces.msg import Time as TimeMsg
 from dynamic_stack_decider.dsd import DSD
 from game_controller_hsl_interfaces.msg import GameState
 from geometry_msgs.msg import PoseWithCovarianceStamped, Twist
@@ -75,6 +76,13 @@ def __init__(self, node: Node):
             qos_profile=1,
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
+        node.create_subscription(
+            TimeMsg,
+            "whistle_detected",
+            blackboard.gamestate.whistle_detection_callback,
+            qos_profile=1,
+            callback_group=MutuallyExclusiveCallbackGroup(),
+        )
 
     def loop(self):
         try:

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/body_behavior.py

@@ -30,16 +30,17 @@
   <depend>soccer_vision_3d_msgs</depend>
   <depend>tf_transformations</depend>
   <depend>tf2</depend>
-  <test_depend>ament_mypy</test_depend>
+  <depend>std_msgs</depend>
+    <test_depend>ament_mypy</test_depend>
 
-  <test_depend>python3-pytest</test_depend>
+    <test_depend>python3-pytest</test_depend>
 
-  <export>
-    <rosdoc config="rosdoc.yaml"/>
-    <bitbots_documentation>
-      <status>starts</status>
-      <language>python3</language>
-    </bitbots_documentation>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
+    <export>
+      <rosdoc config="rosdoc.yaml"/>
+      <bitbots_documentation>
+        <status>starts</status>
+        <language>python3</language>
+      </bitbots_documentation>
+      <build_type>ament_python</build_type>
+    </export>
+  </package>

src/bitbots_behavior/bitbots_body_behavior/package.xml

@@ -1,29 +1,14 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="sim" default="false" description="If true, we don't capture audio" />
-    <arg name="dst" default="appsink"/>
-    <arg name="device" default=""/>
-    <arg name="format" default="wave"/>
-    <arg name="bitrate" default="128"/>
-    <arg name="channels" default="1"/>
-    <arg name="depth" default="16"/>
-    <arg name="sample_rate" default="10000"/>
-    <arg name="sample_format" default="S16LE"/>
-    <arg name="ns" default="audio"/>
-    <arg name="audio_topic" default="audio" />
+    <!-- Our code currently is not sample_rate agnostic, but this must be overwritable to start the audio_capturer_node
+    on some audio drivers/devices if they do not support the default sample_rate of 16000. -->
+    <arg name="sample_rate" default="16000" description="The sample_rate with which the audio should be captured"/>
 
-    <group unless="$(var sim)">
-        <include file="$(find-pkg-share audio_capture)/launch/capture.launch.xml">
-            <arg name="dst" value="$(var dst)"/>
-            <arg name="device" value="$(var device)"/>
-            <arg name="format" value="$(var format)"/>
-            <arg name="bitrate" value="$(var bitrate)"/>
-            <arg name="channels" value="$(var channels)"/>
-            <arg name="depth" value="$(var depth)"/>
-            <arg name="sample_rate" value="$(var sample_rate)"/>
-            <arg name="sample_format" value="$(var sample_format)"/>
-            <arg name="ns" value="$(var ns)"/>
-            <arg name="audio_topic" value="$(var audio_topic)"/>
-        </include>
-    </group>
+    <node
+        pkg="audio_common"
+        exec="audio_capturer_node"
+        name="audio_capturer_node"
+        output="screen">
+        <param name="rate" value="$(var sample_rate)"/>
+    </node>
 </launch>

src/bitbots_misc/bitbots_bringup/launch/README.md

@@ -12,6 +12,7 @@
     <arg name="vision" default="true" description="Whether the vision system should be started" />
     <arg name="workspace_status" default="true" description="Whether to publish the current workspace status"/>
     <arg name="world_model" default="true" description="Whether the world model should be started"/>
+    <arg name="whistle_detector" default="true" description="Whether whistle detector should be started"/>
 
 
     <!-- launch game controller -->
@@ -55,7 +56,9 @@
             <arg name="sim" value="$(var sim)"/>
         </include>
     </group>
-
+    <group if="$(var whistle_detector)">
+        <include file="$(find-pkg-share bitbots_whistle_detector)/launch/whistle_detector.launch"/>
+    </group>
     <!-- launch localization or fake localization -->
     <group if="$(var localization)">
         <include file="$(find-pkg-share bitbots_localization)/launch/localization.launch">
@@ -87,7 +90,6 @@
     <!-- launch audio processing -->
     <group if="$(var audio)">
         <include file="$(find-pkg-share bitbots_bringup)/launch/audio.launch">
-            <arg name="sim" value="$(var sim)" />
         </include>
     </group>