feat(behavior): descend with goalie on block/role-position

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/dynup_animation.py

@@ -0,0 +1,142 @@
+import time
+from abc import ABC, abstractmethod
+
+import rclpy
+from bitbots_blackboard.body_blackboard import BodyBlackboard
+from dynamic_stack_decider.abstract_action_element import AbstractActionElement
+from rclpy.task import Future
+
+from bitbots_msgs.action import Dynup
+
+
+# @TODO: merge/extract with hcm PlayAnimationDynup
+class AbstractDynupAnimation(AbstractActionElement, ABC):
+    """
+    Dynup animation actions are blocking and do not pop themselves!
+    This is because otherwise they would, reset themselves directly (e.g. after descend, ascend again)
+    """
+
+    blackboard: BodyBlackboard
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.active = False
+        self.first_perform = True
+        self.dynup_action_current_goal: Future | None = None
+
+    def perform(self, reevaluate=False):
+        if self.first_perform:
+            # try to start animation
+            success = self.start_animation()
+            # if we fail, we need to abort this action
+            if not success:
+                self.blackboard.node.get_logger().error("Could not start animation. Will abort animation action!")
+                self.pop()
+
+            self.first_perform = False
+
+    @abstractmethod
+    def reset_animation(self):
+        """
+        This method needs to reset the animation to its initial state.
+        For example, if the animation is descend, the robot should stand up again.
+        """
+        raise NotImplementedError
+
+    def start_animation(self) -> bool:
+        """
+        This will NOT wait by itself. You have to check animation_finished() by yourself.
+
+        :return: True if the animation was started, False if not
+        """
+        if not self.is_server_running():
+            return False
+
+        # Dynup action server is running, we can start the walkready action
+        self.send_animation_goal(self.direction)
+        return True
+
+    def send_animation_goal(self, direction: str):
+        goal = Dynup.Goal()
+        goal.direction = direction
+        self.active = True
+
+        self.dynup_action_current_goal = self.blackboard.animation.dynup_action_client.send_goal_async(goal)
+        self.dynup_action_current_goal.add_done_callback(self.animation_finished_callback)
+
+    def is_server_running(self) -> bool:
+        server_running = self.blackboard.animation.dynup_action_client.wait_for_server(timeout_sec=1)
+        if not server_running:
+            while not server_running and rclpy.ok():
+                self.blackboard.node.get_logger().warn(
+                    "Dynup Action Server not running! Dynup cannot work without dynup server! "
+                    "Will now wait until server is accessible!",
+                    throttle_duration_sec=10.0,
+                )
+                server_running = self.blackboard.animation.dynup_action_client.wait_for_server(timeout_sec=1)
+            if server_running:
+                self.blackboard.node.get_logger().warn("Dynup server now running, 'DynupAnimation' action will go on.")
+            else:
+                self.blackboard.node.get_logger().warn("Dynup server did not start.")
+                return False
+
+        return server_running
+
+    def stop_animation(self):
+        if self.dynup_action_current_goal is not None:
+            self.dynup_action_current_goal.result().cancel_goal_async()
+
+    def on_pop(self):
+        """
+        Cancel the current goal when the action is popped
+        """
+        super().on_pop()
+        if not self.is_animation_finished():
+            self.stop_animation()
+
+        self.set_inactive()
+        self.reset_animation()
+
+    def animation_finished_callback(self, animation_done_future: Future):
+        """
+        Dynup animation future callback, setting the action when the animation is finished
+        """
+        animation_done_future.result().get_result_async().add_done_callback(lambda _: self.set_inactive())
+
+    def is_animation_finished(self) -> bool:
+        return not self.active
+
+    def set_inactive(self):
+        self.active = False
+
+    def set_active(self):
+        self.active = True
+
+
+class Descend(AbstractDynupAnimation):
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.direction = Dynup.Goal.DIRECTION_DESCEND
+        self.reset_direction = Dynup.Goal.DIRECTION_RISE
+
+    def reset_animation(self):
+        self.set_active()
+        self.send_animation_goal(self.reset_direction)
+
+        while not self.is_animation_finished():
+            time.sleep(0.1)
+
+
+class GetWalkready(AbstractDynupAnimation):
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.direction = Dynup.Goal.DIRECTION_WALKREADY
+
+    def reset_animation(self):
+        pass
+
+    def perform(self, reevaluate=False):
+        super().perform(reevaluate)
+
+        if self.is_animation_finished():
+            self.pop()

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_block_position.py

@@ -12,6 +12,7 @@ class GoToBlockPosition(AbstractActionElement):
 
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
+        self.blocking = parameters.get("blocking", True)
         self.block_position_goal_offset = self.blackboard.config["block_position_goal_offset"]
         self.block_radius = self.blackboard.config["block_radius_robot"]
         self.left_goalpost_position = [
@@ -64,13 +65,16 @@ def perform(self, reevaluate=False):
 
         self.blackboard.pathfinding.publish(pose_msg)
 
+        if not self.blocking:
+            self.pop()
+
     def _calc_opening_angle(self, ball_to_line: float, ball_pos: tuple) -> float:
         """
         Calculates the opening angle of the ball to both goalposts.
         With it we can get the angle bisector, in which we place the robot.
         Args:
             ball_to_line: distance of the ball to our goal line
-            ball_pos: ball position in world koordinate system
+            ball_pos: ball position in world coordinate system
 
         Returns:
             float: opening angle

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/on_position.py

@@ -0,0 +1,44 @@
+import math
+
+import numpy as np
+from bitbots_blackboard.body_blackboard import BodyBlackboard
+from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement
+from ros2_numpy import numpify
+from tf_transformations import euler_from_quaternion
+
+from bitbots_body_behavior.behavior_dsd.actions.go_to_block_position import GoToBlockPosition
+
+
+class OnBlockPosition(AbstractDecisionElement):
+    blackboard: BodyBlackboard
+
+    def __init__(self, blackboard, dsd, parameters):
+        super().__init__(blackboard, dsd, parameters)
+        self.go_to_block_postion_action = GoToBlockPosition(blackboard, dsd, parameters)
+        self.threshold = parameters.get("threshold", 0.0)
+        self.orientation_threshold = math.radians(self.blackboard.config["goal_alignment_orientation_threshold"])
+
+    def perform(self, reevaluate=False):
+        """
+        Determines whether we are on the block position as goalie
+        """
+        self.go_to_block_postion_action.perform()
+        current_pose = self.blackboard.world_model.get_current_position_pose_stamped()
+        goal_pose = self.blackboard.pathfinding.get_goal()
+
+        if current_pose is None or goal_pose is None:
+            return "NO"
+
+        current_orientation = euler_from_quaternion(numpify(current_pose.pose.orientation))
+        goal_orientation = euler_from_quaternion(numpify(goal_pose.pose.orientation))
+        angle_to_goal_orientation = abs(math.remainder(current_orientation[2] - goal_orientation[2], math.tau))
+        self.publish_debug_data("current_orientation", current_orientation[2])
+        self.publish_debug_data("goal_orientation", goal_orientation[2])
+        self.publish_debug_data("angle_to_goal_orientation", angle_to_goal_orientation)
+
+        distance = np.linalg.norm(numpify(goal_pose.pose.position) - numpify(current_pose.pose.position))
+        self.publish_debug_data("distance", distance)
+
+        if distance < self.threshold and angle_to_goal_orientation < self.orientation_threshold:
+            return "YES"
+        return "NO"

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/reached_goal.py

@@ -83,7 +83,9 @@ class ReachedAndAlignedToPathPlanningGoalPosition(AbstractDecisionElement):
     def __init__(self, blackboard, dsd, parameters):
         super().__init__(blackboard, dsd, parameters)
         self.frame_id = parameters.get("frame_id", self.blackboard.map_frame)
+        # on the map frame the threshold is in meters
         self.threshold = parameters.get("threshold")
+        # the orientation threshold is in degrees
         self.orientation_threshold = math.radians(self.blackboard.config["goal_alignment_orientation_threshold"])
         self.latch = parameters.get("latch", False)
         self.latched = False

bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd

@@ -17,6 +17,9 @@ $DoOnce
 #StandAndLook
 @ChangeAction + action:waiting, @LookAtFieldFeatures, @Stand
 
+#DescendAndLook
+@ChangeAction + action:waiting, @LookAtFieldFeatures, @Stand + duration:1.0, @Descend
+
 #GetWalkreadyAndLocalize
 @ChangeAction + action:waiting + r:false, @PlayAnimationInitInSim + r:false, @LookAtFieldFeatures + r:false, @GetWalkready + r:false, @WalkInPlace
 
@@ -33,7 +36,9 @@ $DoOnce
 $DoOnce
     NOT_DONE --> @LookAtFieldFeatures, @ChangeAction + action:positioning, @GoToRolePosition + blocking:false
     DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:0.2 + latch:true
-        YES -->  #StandAndLook
+        YES --> $ConfigRole
+            GOALIE --> #DescendAndLook
+            ELSE --> #StandAndLook
         NO --> @LookAtFieldFeatures, @GoToRolePosition
 
 #KickWithAvoidance
@@ -61,7 +66,11 @@ $GoalScoreRecently
 #GoalieBehavior
 $ClosestToBall
     NO --> $BallInOwnPercent + p:40
-        YES --> @ChangeAction + action:positioning, @AvoidBallActive, @LookAtFieldFeatures, @GoToBlockPosition
+        YES --> $DoOnce
+            NOT_DONE --> @ChangeAction + action:positioning, @LookAtFieldFeatures, @GoToBlockPosition + blocking:false
+            DONE --> $OnBlockPosition + threshold:0.2
+                YES --> @Stand + duration:1.0, @Descend
+                NO --> @GoToBlockPosition + blocking:false
         NO --> #RolePositionWithPause
     YES --> #KickWithAvoidance
 

bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py

@@ -26,10 +26,10 @@ def perform(self, reevaluate=False):
             anim = self.choose_animation()
 
             # try to start animation
-            sucess = self.start_animation(anim)
+            success = self.start_animation(anim)
 
             # if we fail, we need to abort this action
-            if not sucess:
+            if not success:
                 self.blackboard.node.get_logger().error("Could not start animation. Will abort play animation action!")
                 return self.pop()
 
@@ -208,7 +208,8 @@ def on_pop(self):
     def start_animation(self):
         """
         This will NOT wait by itself. You have to check animation_finished() by yourself.
-        :return:
+
+        :return: True if the animation was started, False if not
         """
 
         first_try = self.blackboard.dynup_action_client.wait_for_server(