decide for kicking depending on distance of seen robots

.vscode/settings.json

@@ -127,6 +127,7 @@
         "tqdm",
         "unpenalize",
         "unpenalized",
+        "upfield",
         "urdf",
         "vcstool",
         "vram",

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py

@@ -41,6 +41,9 @@ 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"]
+        # upfield is towards the opponent goal, downfield is towards our own goal
+        self.closest_robot_upfield_dist: float = 10000.0
+        self.closest_robot_downfield_dist: float = 10000.0
 
         # Publisher for visualization in RViZ
         self.costmap_publisher = self._node.create_publisher(OccupancyGrid, "debug/costmap", 1)
@@ -61,6 +64,8 @@ def robot_callback(self, msg: RobotArray) -> None:
         """
         # Init a new obstacle costmap
         obstacle_map = np.zeros_like(self.costmap)
+        self.closest_robot_upfield_dist = 10000.0
+        self.closest_robot_downfield_dist = 10000.0
         # Iterate over all robots
         robot: Robot
         for robot in msg.robots:
@@ -69,6 +74,25 @@ 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 = float(
+                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_upfield_dist:
+                    self.closest_robot_upfield_dist = dist_to_robot
+            else:
+                if dist_to_robot < self.closest_robot_downfield_dist:
+                    self.closest_robot_downfield_dist = dist_to_robot
+
         # Smooth obstacle map
         obstacle_map = gaussian_filter(obstacle_map, self.obstacle_costmap_smoothing_sigma)
         # Get pass offsets
@@ -432,3 +456,9 @@ def get_best_kick_direction(
             )
         ]
         return kick_direction
+
+    def is_other_robot_close(self, threshold_upfield: float, threshold_downfield: float) -> bool:
+        return (
+            threshold_upfield > self.closest_robot_upfield_dist
+            or threshold_downfield > self.closest_robot_downfield_dist
+        )

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

@@ -2,22 +2,37 @@
 
 from bitbots_blackboard.body_blackboard import BodyBlackboard
 from dynamic_stack_decider.abstract_decision_element import AbstractDecisionElement
+from game_controller_hsl_interfaces.msg import GameState
 
 
 class KickOrDribble(AbstractDecisionElement):
     blackboard: BodyBlackboard
 
     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)
+        # upfield is towards the opponent goal, downfield is towards our own goal
+        self.threshold_upfield = parameters.get("threshold_upfield", 10)
+        self.threshold_downfield = parameters.get("threshold_downfield", 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
+        # Get the map goal, so we can check the angle.
+        # distance or side offset are not relevant here, so we can just use 0.0 for both.
+        map_goal = self.blackboard.pathfinding.get_map_goal(distance=0.0, side_offset=0.0)
+
+        # Are no other robots too close?
+        other_robots_close = self.blackboard.costmap.is_other_robot_close(
+            self.threshold_upfield, self.threshold_downfield
+        )
+        # Get actual set play situation
+        set_play_state = self.blackboard.gamestate.get_set_play()
+
+        map_goal_points_away_from_opp_goal = abs(map_goal[2]) > math.pi / 2
+        regular_play_with_nearby_robot = other_robots_close and set_play_state == GameState.SET_PLAY_NONE
+
+        if map_goal_points_away_from_opp_goal or regular_play_with_nearby_robot:
             return "DRIBBLE"
         else:
             return "KICK"

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 + threshold_upfield:2.0 + threshold_downfield: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