Merge branch 'feature/whistle_detection' into go-first-game

Author: texhnolyze

.gitignore

@@ -51,7 +51,6 @@ fabric.properties
 ### ROS template
 build/
 bin/
-lib/
 msg_gen/
 srv_gen/
 msg/*Action.msg

pixi.lock

@@ -165,7 +165,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"
@@ -197,6 +196,12 @@ setuptools = "==68.1.2"  # Pinning setuptools to avoid issues with colcon build
 git = ">=2.52.0,<3"  # Otherwise the system git might be used causing ABI issues
 curl = ">=8.17.0,<9"
 make = ">=4.4.1,<5"  # To avoid issues in the CI
+libflac = "==1.4.3"
+portaudio = ">=19.7.0,<20"
+ros-jazzy-ament-cmake-clang-format = ">=0.17.3,<0.18"
+ros-jazzy-rosidl-runtime-c = ">=4.6.7,<5"
+alsa-plugins = ">=1.2.12,<2"
+git-subrepo = ">=0.4.9,<0.5"
 
 [feature.ros.target.linux-64.dependencies]
 # For now Webots is only packaged for x86-64 Linux
@@ -233,6 +238,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 = [
@@ -244,3 +250,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

@@ -118,8 +118,8 @@ def _parse_arguments(self) -> argparse.Namespace:
         parser.add_argument(
             "-w",
             "--workspace",
-            default=".",
-            help="Path to the workspace directory to deploy to. Defaults to the current directory.",
+            default="~/bitbots_main",
+            help="Path to the workspace directory to deploy to. Defaults to 'bitbots_main' in $HOME dir.",
         )
         parser.add_argument("--skip-local-repo-check", action="store_true", help="Skip the local repository check.")
 

scripts/deploy/deploy_robots.py

@@ -21,6 +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.game_controller_stop: bool = False
         # publish stopped msg for hcm
         self.stop_pub = node.create_publisher(Bool, "game_controller/stop_msg", 1)
@@ -135,3 +136,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) -> None:  # MSG type is assumed to be bool for now, prototyping
+        self.whistle_detected = msg.data  # This stays true after the first whistle detection which is incorrect

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

@@ -0,0 +1,19 @@
+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.bb = blackboard
+        self.bb.gamestate.whistle_detected = False
+
+    def perform(self, reevaluate=False):
+        if self.bb.gamestate.whistle_detected: 
+            return "YES"
+        return "NO"
+
+    def get_reevaluate(self):
+        return True

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

@@ -136,6 +136,16 @@ $BallSeen
                     SECOND --> #SupporterRole
                     THIRD --> #DefensePositioning
 
+#PlayingBehavior
+$SecondaryStateDecider
+    PENALTYSHOOT --> #PenaltyShootoutBehavior
+    TIMEOUT --> #StandAndLook
+    ELSE --> $SecondaryStateTeamDecider
+        OUR --> #NormalBehavior // Nutzt immer normalBehaviour wenn wir die aktion ausführen, könnte man spezifischeres Verhalten definieren
+        OTHER --> #Placing
+    NORMAL --> #NormalBehavior
+    OVERTIME --> #NormalBehavior
+
 -->BodyBehavior
 $IsPenalized
     YES --> #DoNothing
@@ -149,23 +159,17 @@ $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 // 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
+        SET --> $WhistleDetected
+            YES --> #PlayingBehavior
+            NO --> $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 //Nutzt immer normalBehaviour wenn wir die aktion ausführen, könnte man spezifischeres Verhalten definieren 
-                OTHER --> #Placing
-            NORMAL --> #NormalBehavior
-            OVERTIME --> #NormalBehavior
+        PLAYING --> #PlayingBehavior
         STANDBY --> #StandAndLook
-            

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

@@ -11,6 +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 bitbots_body_behavior import behavior_dsd
 from bitbots_msgs.msg import RobotControlState, TeamData
@@ -75,6 +76,13 @@ def __init__(self, node: Node):
             qos_profile=1,
             callback_group=MutuallyExclusiveCallbackGroup(),
         )
+        node.create_subscription(
+            Bool,
+            "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,7 +1,7 @@
 #ifndef BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_BITFOOT_HARDWARE_INTERFACE_H_
 #define BITBOTS_ROS_CONTROL_INCLUDE_BITBOTS_ROS_CONTROL_BITFOOT_HARDWARE_INTERFACE_H_
 
-#include <dynamixel_driver.h>
+#include <dynamixel_workbench_toolbox/dynamixel_driver.h>
 
 #include <bitbots_msgs/msg/foot_pressure.hpp>
 #include <bitbots_ros_control/hardware_interface.hpp>