Fix opponent back line positioning (#488)

Author: val-ba

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/costmap_capsule.py

@@ -236,6 +236,14 @@ def calc_base_costmap(self):
             [
                 ((self.field_length, self.field_width / 2 - self.goal_width / 2), goalpost_value),
                 ((self.field_length, self.field_width / 2 + self.goal_width / 2), goalpost_value),
+                (
+                    (self.field_length + self.map_margin, self.field_width / 2 - self.goal_width / 2),
+                    corner_value + in_field_value_our_side,
+                ),
+                (
+                    (self.field_length + self.map_margin, self.field_width / 2 + self.goal_width / 2),
+                    corner_value + in_field_value_our_side,
+                ),
                 (
                     (self.field_length, self.field_width / 2 - self.goal_width / 2 + goalpost_safety_distance),
                     goal_value,
@@ -247,14 +255,14 @@ def calc_base_costmap(self):
                 (
                     (
                         self.field_length + self.map_margin,
-                        self.field_width / 2 - self.goal_width / 2 - goalpost_safety_distance,
+                        self.field_width / 2 - self.goal_width / 2 + goalpost_safety_distance,
                     ),
                     -0.2,
                 ),
                 (
                     (
                         self.field_length + self.map_margin,
-                        self.field_width / 2 + self.goal_width / 2 + goalpost_safety_distance,
+                        self.field_width / 2 + self.goal_width / 2 - goalpost_safety_distance,
                     ),
                     -0.2,
                 ),

bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/pathfinding_capsule.py

@@ -165,8 +165,12 @@ def get_ball_goal(self, target: BallGoalType, distance: float) -> PoseStamped:
 
             ball_x, ball_y = self._blackboard.world_model.get_ball_position_xy()
 
+            # Play in any part of the opponents goal, not just the center
             if abs(ball_y) < self._blackboard.world_model.goal_width / 2:
                 goal_angle = 0
+            # Play in the opposite direction if the ball is near the opponent goal backline
+            elif ball_x > self._blackboard.world_model.field_length / 2 - 0.2:
+                goal_angle = math.pi + np.copysign(math.pi / 4, ball_y)
 
             goal_x = ball_x - math.cos(goal_angle) * distance
             goal_y = ball_y - math.sin(goal_angle) * distance

bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -195,7 +195,7 @@
       ##################
 
       # sigma of gaussian blur applied to costmap
-      base_costmap_smoothing_sigma: 0.3
+      base_costmap_smoothing_sigma: 1.0
 
       # margin that is added around the field size when creating the costmap (meters)
       map_margin: 1.0
@@ -207,7 +207,7 @@
       goal_value: 0.0
 
       # cost at a goalpost
-      goalpost_value: 1.0
+      goalpost_value: 0.5
 
       # cost in a corner
       corner_value: 1.0