diff --git a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py index 35b038995..86ed25b83 100644 --- a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py +++ b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py @@ -41,6 +41,8 @@ def __init__(self, node, blackboard): self.map_margin: float = self.body_config["map_margin"] self.obstacle_costmap_smoothing_sigma: float = self.body_config["obstacle_costmap_smoothing_sigma"] self.obstacle_cost: float = self.body_config["obstacle_cost"] + self.closest_robot_infront_dist: float = 10000.0 + self.closest_robot_behind_dist: float = 10000.0 # Publisher for visualization in RViZ self.costmap_publisher = self._node.create_publisher(OccupancyGrid, "debug/costmap", 1) @@ -61,6 +63,8 @@ def robot_callback(self, msg: RobotArray) -> None: """ # Init a new obstacle costmap obstacle_map = np.zeros_like(self.costmap) + self.closest_robot_infront_dist = 10000.0 + self.closest_robot_behind_dist = 10000.0 # Iterate over all robots robot: Robot for robot in msg.robots: @@ -69,6 +73,16 @@ def robot_callback(self, msg: RobotArray) -> None: # TODO inflate # Draw obstacle with smoothing independent weight on obstacle costmap obstacle_map[idx_x, idx_y] = self.obstacle_cost * self.obstacle_costmap_smoothing_sigma + + dist_to_robot = np.linalg.norm(np.array([self._blackboard.world_model.get_current_position()[0], self._blackboard.world_model.get_current_position()[1]]) - np.array([robot.bb.center.position.x,robot.bb.center.position.y])) + if robot.bb.center.position.x > self._blackboard.world_model.get_current_position()[0]: + if dist_to_robot < self.closest_robot_infront_dist: + self.closest_robot_infront_dist = dist_to_robot + else: + if dist_to_robot < self.closest_robot_behind_dist: + self.closest_robot_behind_dist = dist_to_robot + + # Smooth obstacle map obstacle_map = gaussian_filter(obstacle_map, self.obstacle_costmap_smoothing_sigma) # Get pass offsets @@ -432,3 +446,11 @@ def get_best_kick_direction( ) ] return kick_direction + + def is_other_robot_close(self, threshold_front: float, threshold_behind: float) -> bool: + if threshold_front > self.closest_robot_infront_dist: + return True + elif threshold_behind > self.closest_robot_behind_dist: + return True + else: + return False diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/dribble_or_kick.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/dribble_or_kick.py index 45db9efe0..93013031d 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/dribble_or_kick.py +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/dribble_or_kick.py @@ -17,6 +17,8 @@ def __init__(self, blackboard, dsd, parameters): self.num_kick_angles = self.blackboard.config["num_kick_angles"] self.dribble_kick_angle = self.blackboard.config["dribble_kick_angle"] + self.threshold_front = parameters.get("threshold_front", 10) + self.threshold_behind = parameters.get("threshold_behind", 10) def perform(self, reevaluate=False): """ @@ -50,7 +52,12 @@ def perform(self, reevaluate=False): ball_near = ball_distance < self.ball_distance_threshold self.publish_debug_data(f"Ball distance (needs <{self.ball_distance_threshold})", ball_distance) - if oriented_to_goal and front_free and goal_far and ball_near: + # no other robots to close + other_robots_close = self.blackboard.costmap.is_other_robot_close(self.threshold_front, self.threshold_behind) + # actual set play situation + set_play_state = self.blackboard.gamestate.get_set_play() + + if other_robots_close and set_play_state == 0: return "DRIBBLE" else: return "KICK" diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/kick_or_dribble.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/kick_or_dribble.py index d25c2ae1d..1c3fa0107 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/kick_or_dribble.py +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/decisions/kick_or_dribble.py @@ -11,13 +11,20 @@ def __init__(self, blackboard, dsd, parameters): super().__init__(blackboard, dsd, parameters) self.target_distance = parameters.get("map_goal_target_distance", 0.5) self.side_offset = parameters.get("side_offset", 0.0) + self.threshold_front = parameters.get("threshold_front", 10) + self.threshold_behind = parameters.get("threshold_behind", 10) def perform(self, reevaluate=False): """ Determines whether to kick or dribble based on the angle of the map goal """ map_goal = self.blackboard.pathfinding.get_map_goal(self.target_distance, self.side_offset) - if abs(map_goal[2]) > math.pi / 2: # point away from opponent goal, so we should dribble + # no other robots to close + other_robots_close = self.blackboard.costmap.is_other_robot_close(self.threshold_front, self.threshold_behind) + # actual set play situation + set_play_state = self.blackboard.gamestate.get_set_play() + + if abs(map_goal[2]) > math.pi / 2 or (other_robots_close and set_play_state == 0): # point away from opponent goal, so we should dribble return "DRIBBLE" else: return "KICK" diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd index 7851fbf80..010a193c2 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd +++ b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/main.dsd @@ -31,7 +31,7 @@ $DoOnce NO --> @LookAtFieldFeatures, @GoToRolePosition #KickWithAvoidance -$KickOrDribble + map_goal_distance:%ball_approach_dist // should be same as below $GoToBall + target:map action +$KickOrDribble + map_goal_distance:%ball_approach_dist + threshold_front:2.0 + threshold_behind:0.5 KICK --> $DoOnce NOT_DONE --> @ChangeAction + action:going_to_ball, @LookAtFieldFeatures, @GoToBall + target:rl_kick + blocking:false DONE --> $ReachedAndAlignedToPathPlanningGoalPosition + threshold:%rl_kick_approach_tolerance_pos + orientation_threshold:%rl_kick_approach_tolerance_deg + latch:true