diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/dynup_animation.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/dynup_animation.py new file mode 100644 index 000000000..75e96a723 --- /dev/null +++ b/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() diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/get_walkready.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/get_walkready.py deleted file mode 100644 index 0dfd33caf..000000000 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/get_walkready.py +++ /dev/null @@ -1,80 +0,0 @@ -import rclpy -from bitbots_blackboard.body_blackboard import BodyBlackboard -from dynamic_stack_decider.abstract_action_element import AbstractActionElement - -from bitbots_msgs.action import Dynup - - -class GetWalkready(AbstractActionElement): - blackboard: BodyBlackboard - - def __init__(self, blackboard, dsd, parameters): - super().__init__(blackboard, dsd, parameters) - self.first_perform = True - self.active = False - - def perform(self, reevaluate=False): - # deactivate falling since it will be wrongly detected - self.do_not_reevaluate() - if self.first_perform: - # get the animation that should be played - # defined by implementations of this abstract class - - # 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 play animation action!") - return self.pop() - - self.first_perform = False - return - - if self.animation_finished(): - # we are finished playing this animation - return self.pop() - - 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 - """ - server_running = self.blackboard.animation.dynup_action_client.wait_for_server(timeout_sec=1.0) - 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, 'get_walkready' action will go on.") - else: - self.blackboard.node.get_logger().warn("Dynup server did not start.") - return False - - # Dynup action server is running, we can start the walkready action - goal = Dynup.Goal() - goal.direction = Dynup.Goal.DIRECTION_WALKREADY - 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( - lambda future: future.result() - .get_result_async() - .add_done_callback(lambda result_future: self.__done_cb(result_future)) - ) - return True - - def __done_cb(self, result_future): - self.active = False - - def animation_finished(self) -> bool: - """ - Checks if the animation is finished. - - :return: True if the animation is finished, False if not - """ - return not self.active diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_block_position.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_block_position.py index c2f91fc90..0a932a030 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_block_position.py +++ b/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 diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/on_position.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/on_position.py new file mode 100644 index 000000000..73085c455 --- /dev/null +++ b/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" diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/reached_goal.py b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/reached_goal.py index a29b12a5b..0365bc707 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/reached_goal.py +++ b/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 diff --git a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd b/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd index c71286d92..e264857d2 100644 --- a/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd +++ b/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 diff --git a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py b/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py index 8f805b1da..8c37041d7 100644 --- a/bitbots_motion/bitbots_hcm/bitbots_hcm/hcm_dsd/actions/play_animation.py +++ b/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(