fix: use full-range field maps and field-aware kickoff positioning

src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py

@@ -39,12 +39,14 @@ def __init__(self, node, blackboard):
             "/parameter_blackboard",
             [
                 "field.goal.width",
+                "field.markings.center_circle.diameter",
                 "field.markings.penalty_area.size.x",
                 "field.size.x",
                 "field.size.y",
             ],
         )
         self.goal_width: float = parameters["field.goal.width"]
+        self.center_circle_diameter: float = parameters["field.markings.center_circle.diameter"]
         self.penalty_area_size_x: float = parameters["field.markings.penalty_area.size.x"]
         self.field_length: float = parameters["field.size.x"]
         self.field_width: float = parameters["field.size.y"]

src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_role_position.py

@@ -1,5 +1,6 @@
 from bitbots_blackboard.body_blackboard import BodyBlackboard
 from dynamic_stack_decider.abstract_action_element import AbstractActionElement
+from game_controller_hsl_interfaces.msg import GameState
 from tf2_geometry_msgs import PoseStamped
 
 
@@ -28,6 +29,22 @@ def __init__(self, blackboard, dsd, parameters):
             generalized_role_position[1] * self.blackboard.world_model.field_width / 2,
         ]
 
+        # The central striker waits behind the circle at an opponent kickoff.
+        if (
+            self.blackboard.gamestate.get_main_state() in (GameState.STATE_READY, GameState.STATE_SET)
+            and self.blackboard.gamestate.get_set_play() == GameState.SET_PLAY_NONE
+            and self.blackboard.gamestate.get_game_phase()
+            in (GameState.GAME_PHASE_NORMAL, GameState.GAME_PHASE_EXTRA_TIME)
+            and kickoff_type == "passive"
+            and self.blackboard.team_data.role == "offense"
+            and self.blackboard.misc.position_number == 0
+        ):
+            kickoff_striker_distance = (
+                self.blackboard.world_model.center_circle_diameter / 2
+                + self.blackboard.config["kickoff_striker_circle_margin"]
+            )
+            self.role_position = [-kickoff_striker_distance, 0.0]
+
         self.blocking = parameters.get("blocking", True)
 
     def perform(self, reevaluate=False):

src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml

@@ -24,6 +24,9 @@ body_behavior:
     #                         0
     # To be useful for different field sizes, use values in [-1, 1] for x and y
 
+    # Distance behind the center-circle boundary for the central striker at an opponent kickoff, in meters.
+    kickoff_striker_circle_margin: 0.4
+
     role_positions:
       goalie: [ -0.9, 0.0 ]
       defense:

src/bitbots_behavior/bitbots_body_behavior/test/test_kickoff_position.py

@@ -0,0 +1,77 @@
+"""Only opponent kickoff positioning uses the center-circle diameter."""
+
+from pathlib import Path
+from types import SimpleNamespace
+from unittest.mock import Mock
+
+import pytest
+import yaml  # type: ignore[import-untyped]
+from bitbots_body_behavior.behavior_dsd.actions.go_to_role_position import GoToRolePosition
+from game_controller_hsl_interfaces.msg import GameState
+
+
+def make_blackboard(
+    diameter=1.5,
+    role="offense",
+    position=0,
+    has_kick=False,
+    state=GameState.STATE_READY,
+    set_play=GameState.SET_PLAY_NONE,
+    phase=GameState.GAME_PHASE_NORMAL,
+):
+    config = yaml.safe_load((Path(__file__).parents[1] / "config/body_behavior.yaml").read_text())
+    config = config["body_behavior"]["ros__parameters"]
+    for name in ("offense", "defense"):
+        for kickoff in ("active", "passive"):
+            config["role_positions"][name][kickoff] = {
+                str(key): value for key, value in config["role_positions"][name][kickoff].items()
+            }
+    return SimpleNamespace(
+        config=config,
+        gamestate=SimpleNamespace(
+            has_kick=lambda: has_kick,
+            get_main_state=lambda: state,
+            get_set_play=lambda: set_play,
+            get_game_phase=lambda: phase,
+        ),
+        team_data=SimpleNamespace(role=role),
+        misc=SimpleNamespace(position_number=position),
+        world_model=SimpleNamespace(field_length=14.0, field_width=9.0, center_circle_diameter=diameter),
+    )
+
+
+@pytest.mark.parametrize("diameter", [1.2, 1.5, 3.0])
+@pytest.mark.parametrize("state", [GameState.STATE_READY, GameState.STATE_SET])
+def test_opponent_kickoff_striker(diameter, state):
+    blackboard = make_blackboard(diameter=diameter, state=state)
+    blackboard.config["kickoff_striker_circle_margin"] = 0.35
+    action = GoToRolePosition(blackboard, Mock(), {})
+    assert action.role_position == pytest.approx([-diameter / 2 - 0.35, 0.0])
+
+
+@pytest.mark.parametrize(
+    "kwargs",
+    [
+        {"has_kick": True},
+        {"role": "goalie"},
+        {"role": "defense"},
+        {"role": "defense", "position": 1},
+        {"role": "defense", "position": 2},
+        {"position": 1},
+        {"position": 2},
+        {"state": GameState.STATE_PLAYING},
+        {"set_play": GameState.SET_PLAY_DIRECT_FREE_KICK},
+        {"set_play": GameState.SET_PLAY_CORNER_KICK},
+        {"phase": GameState.GAME_PHASE_PENALTY_SHOOT_OUT},
+    ],
+)
+def test_other_positions_keep_configured_target(kwargs):
+    blackboard = make_blackboard(diameter=3.0, **kwargs)
+    role = blackboard.team_data.role
+    target = blackboard.config["role_positions"][role]
+    if role != "goalie":
+        target = target["active" if blackboard.gamestate.has_kick() else "passive"][
+            str(blackboard.misc.position_number)
+        ]
+    expected = [target[0] * blackboard.world_model.field_length / 2, target[1] * blackboard.world_model.field_width / 2]
+    assert GoToRolePosition(blackboard, Mock(), {}).role_position == expected

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/demo/config.yaml

@@ -14,6 +14,8 @@ parameter_blackboard:
         y: 1000.0
         padding: 0.1   # Padding (area outside of the lines) of the field in all directions
       markings:
+        center_circle:
+          diameter: 0.0
         penalty_area:
           size:
             x: 0.75   # The distance from the goal line to the boundary of the penalty area

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2024/config.yaml

@@ -12,6 +12,8 @@ parameter_blackboard:
         y: 6.0
         padding: 1.0   # Padding (area outside of the lines) of the field in all directions
       markings:
+        center_circle:
+          diameter: 1.5
         penalty_area:
           size:
             x: 2.0   # The distance from the goal line to the boundary of the penalty area

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2026_medium/config.yaml

@@ -12,6 +12,8 @@ parameter_blackboard:
         y: 5.94
         padding: 1.0   # Padding (area outside of the lines) of the field in all directions
       markings:
+        center_circle:
+          diameter: 1.5
         penalty_area:
           size:
             x: 1.93   # The distance from the goal line to the boundary of the penalty area

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/config.yaml

@@ -12,6 +12,8 @@ parameter_blackboard:
         y: 6.0
         padding: 1.0   # Padding (area outside of the lines) of the field in all directions
       markings:
+        center_circle:
+          diameter: 1.5
         penalty_area:
           size:
             x: 2.0   # The distance from the goal line to the boundary of the penalty area

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid.yaml

@@ -24,6 +24,8 @@ parameters:
   field_feature_size: 10
   mark_type: cross
   field_feature_style: exact
-  distance_map: false
+  distance_map: true
   distance_decay: 0.0
   invert: true
+  grayscale_min: 0
+  grayscale_max: 255

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/config.yaml

@@ -12,6 +12,8 @@ parameter_blackboard:
         y: 5.01
         padding: 0.8   # Padding (area outside of the lines) of the field in all directions
       markings:
+        center_circle:
+          diameter: 1.3
         penalty_area:
           size:
             x: 1.65   # The distance from the goal line to the boundary of the penalty area

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/gui-config.yaml

@@ -27,3 +27,5 @@ parameters:
   distance_map: true
   distance_decay: 0.0
   invert: true
+  grayscale_min: 0
+  grayscale_max: 255

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/labor/config.yaml

@@ -12,6 +12,8 @@ parameter_blackboard:
         y: 3.88
         padding: 0.1   # Padding (area outside of the lines) of the field in all directions
       markings:
+        center_circle:
+          diameter: 1.2
         penalty_area:
           size:
             x: 0.75   # The distance from the goal line to the boundary of the penalty area

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/config.yaml

@@ -8,6 +8,8 @@ parameter_blackboard:
         y: 9.0
         padding: 1.0   # Padding (area outside of the lines) of the field in all directions
       markings:
+        center_circle:
+          diameter: 3.0
         penalty_area:
           size:
             x: 3.0   # The distance from the goal line to the boundary of the penalty area

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/gui-config.yaml

@@ -27,3 +27,5 @@ parameters:
   distance_map: true
   distance_decay: 0.0
   invert: true
+  grayscale_min: 0
+  grayscale_max: 255

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/config.yaml

@@ -6,6 +6,8 @@ parameter_blackboard:
         y: 6.0
         padding: 1.0   # Padding (area outside of the lines) of the field in all directions
       markings:
+        center_circle:
+          diameter: 1.5
         penalty_area:
           size:
             x: 2.0   # The distance from the goal line to the boundary of the penalty area

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/gui-config.yaml

@@ -24,6 +24,8 @@ parameters:
   field_feature_size: 30
   mark_type: point
   field_feature_style: exact
-  distance_map: false
+  distance_map: true
   distance_decay: 0.0
   invert: true
+  grayscale_min: 0
+  grayscale_max: 255

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/spl/config.yaml

@@ -12,6 +12,8 @@ parameter_blackboard:
         y: 6.0
         padding: 0.7   # Padding (area outside of the lines) of the field in all directions
       markings:
+        center_circle:
+          diameter: 1.5
         penalty_area:
           size:
             x: 1.65   # The distance from the goal line to the boundary of the penalty area

src/bitbots_misc/bitbots_parameter_blackboard/config/fields/webots/config.yaml

@@ -12,6 +12,8 @@ parameter_blackboard:
         y: 6.0
         padding: 1.0   # Padding (area outside of the lines) of the field in all directions
       markings:
+        center_circle:
+          diameter: 1.5
         penalty_area:
           size:
             x: 2.0   # The distance from the goal line to the boundary of the penalty area

src/bitbots_navigation/bitbots_localization/CMakeLists.txt

@@ -122,4 +122,22 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 install(TARGETS localization DESTINATION lib/${PROJECT_NAME})
 
+if(BUILD_TESTING)
+  enable_testing()
+  add_executable(test_map test/test_map.cpp src/map.cpp src/RobotState.cpp
+                          src/tools.cpp)
+  ament_target_dependencies(
+    test_map
+    ament_index_cpp
+    bitbots_utils
+    Boost
+    nav_msgs
+    OpenCV
+    particle_filter
+    rclcpp
+    tf2
+    tf2_geometry_msgs)
+  add_test(NAME map_grayscale COMMAND test_map)
+endif()
+
 ament_package()

src/bitbots_navigation/bitbots_localization/README.md

@@ -1,2 +1,7 @@
 # bitbots_localization
 
+
+Field distance maps use the full unsigned byte grayscale range, with dark pixels
+representing field lines. Scoring normalizes the inverted intensity; published
+occupancy grids convert it to the ROS percentage range. Out-of-field scores
+remain configured as percentages. Regenerate maps from their saved generator inputs when changing field geometry.

src/bitbots_navigation/bitbots_localization/src/localization.cpp

@@ -530,7 +530,7 @@ void Localization::publish_debug_rating(const std::vector<std::pair<double, doub
     std_msgs::msg::ColorRGBA color;
     color.b = 1;
     if (occupancy >= 0) {
-      color.r = occupancy / 100;
+      color.r = occupancy;
     } else {
       color.g = 1;
     }

src/bitbots_navigation/bitbots_localization/src/map.cpp

@@ -43,7 +43,7 @@ double Map::get_occupancy(double x, double y) {
   double occupancy = out_of_map_value_;  // punish points outside the map
 
   if (x < mapWidth && x >= 0 && y < mapHeight && y >= 0) {
-    occupancy = 100 - map.at<uchar>(y, x);
+    return (255 - map.at<uchar>(y, x)) / 255.0;
   }
   return occupancy / 100.0;
 }
@@ -96,7 +96,7 @@ nav_msgs::msg::OccupancyGrid Map::get_map_msg(std::string frame_id, int threshol
   map_msg.data.resize(map.rows * map.cols);
   for (int i = 0; i < map.rows; i++) {
     for (int j = 0; j < map.cols; j++) {
-      map_msg.data[i * map.cols + j] = 100 - map.at<uchar>(i, j);
+      map_msg.data[i * map.cols + j] = std::lround((255 - map.at<uchar>(i, j)) * 100.0 / 255.0);
     }
   }
   return map_msg;

src/bitbots_navigation/bitbots_localization/test/test_map.cpp

@@ -0,0 +1,27 @@
+#include <bitbots_localization/map.hpp>
+#include <cmath>
+#include <iostream>
+
+int main() {
+  bitbots_localization::Map map("hsl_kid", "lines.png", 25.0);
+  map.map = cv::Mat(2, 256, CV_8UC1);
+  for (int x = 0; x < map.map.cols; ++x) {
+    map.map.at<uchar>(0, x) = x;
+    map.map.at<uchar>(1, x) = x;
+  }
+  const auto grid = map.get_map_msg("map");
+  for (int x = 0; x < map.map.cols; ++x) {
+    const double score = map.get_occupancy((x - 128) / 100.0, 0.0);
+    const double expected = 1.0 - x / 255.0;
+    if (std::abs(score - expected) > 1e-12 || grid.data[x] < 0 || grid.data[x] > 100 ||
+        std::abs(grid.data[x] / 100.0 - score) > 0.0051) {
+      std::cerr << "Incorrect score or occupancy grid value for pixel " << x << '\n';
+      return 1;
+    }
+  }
+  if (map.get_occupancy(10.0, 0.0) != 0.25) {
+    std::cerr << "Out-of-map percentage changed\n";
+    return 1;
+  }
+  return 0;
+}