Merge remote-tracking branch 'origin/main' into rl_motion_framework

Author: sWintermoor

pixi.lock

@@ -25,7 +25,7 @@ args = [{ arg = "package", default = "" }]
 description = "Cleans build and install files for all ROS 2 packages or a specific one if given."
 
 [tasks.test]
-cmd = "colcon test --event-handlers console_direct+ --return-code-on-test-failure "
+cmd = "colcon test --packages-ignore=audio_common --event-handlers console_direct+ --return-code-on-test-failure "
 description = "Runs tests for all ROS 2 packages. Add --packages-select <package names> to test specific packages."
 
 [dependencies]
@@ -39,6 +39,7 @@ pkg-config = ">=0.29.2,<0.30"
 
 [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
@@ -53,6 +54,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"
@@ -64,6 +66,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"
@@ -86,6 +89,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"
@@ -133,6 +137,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"
@@ -167,11 +172,11 @@ 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"
-ros-jazzy-game-controller-hl-interfaces = ">=0.0.1,<0.0.2"
+# To be replaced with ros-jazzy-game-controller-hsl once the new version is released upstream
+# ros-jazzy-game-controller-hl = ">=1.1.0,<2"
+# ros-jazzy-game-controller-hl-interfaces = ">=0.0.1,<0.0.2"
 ros-jazzy-humanoid-base-footprint = ">=0.1.2,<0.2"
 ros-jazzy-ipm-image-node = ">=0.0.0,<0.0.1"
 ros-jazzy-ipm-interfaces = ">=0.0.0,<0.0.1"
@@ -240,6 +245,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 = [

pixi.toml

@@ -62,7 +62,8 @@ def _build(self, connections: Group) -> GroupResult:
         :return: The results of the task.
         """
         print_debug("Building packages")
-        cmd = f"cd {self._remote_workspace} && pixi run --environment robot build {self._package}"
+
+        cmd = f"cd {self._remote_workspace} && chrt -r 1 taskset -c 0-9 pixi run --environment robot build {self._package}"
 
         print_debug(f"Calling '{cmd}'")
         try:

scripts/deploy/tasks/build.py

@@ -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

@@ -58,9 +58,10 @@ def __init__(self, node, blackboard):
         self.map_margin: float = self._node.get_parameter("map_margin").value
 
         # Ball state
+        # The ball in the map frame (default to the center of the field if ball is not seen yet)
         self._ball: PointStamped = PointStamped(
-            header=Header(stamp=Time(clock_type=ClockType.ROS_TIME), frame_id=self.map_frame)
-        )  # The ball in the map frame (default to the center of the field if ball is not seen yet)
+            header=Header(stamp=Time(clock_type=ClockType.ROS_TIME).to_msg(), frame_id=self.map_frame)
+        )
         self._ball_covariance: np.ndarray = np.zeros((2, 2))  # Covariance of the ball
 
         # Publisher for visualization in RViZ

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_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>