diff --git a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py b/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py index e4b2696f64..b52ca77697 100644 --- a/src/bitbots_behavior/bitbots_blackboard/bitbots_blackboard/capsules/world_model_capsule.py +++ b/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"] diff --git a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_role_position.py b/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_role_position.py index b7e31edf48..cae92c5100 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/bitbots_body_behavior/behavior_dsd/actions/go_to_role_position.py +++ b/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): diff --git a/src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml b/src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml index 471e728c49..087d469d02 100644 --- a/src/bitbots_behavior/bitbots_body_behavior/config/body_behavior.yaml +++ b/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: diff --git a/src/bitbots_behavior/bitbots_body_behavior/test/test_kickoff_position.py b/src/bitbots_behavior/bitbots_body_behavior/test/test_kickoff_position.py new file mode 100644 index 0000000000..a86b215f9b --- /dev/null +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/demo/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/demo/config.yaml index bad1c3eada..954f7755d0 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/demo/config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2024/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2024/config.yaml index af83de08a6..1866a6d14d 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2024/config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2024/lines.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2024/lines.png index 9f8e8ae858..2609bb4d41 100644 Binary files a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2024/lines.png and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2024/lines.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2026_medium/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2026_medium/config.yaml index 469a07db39..79d0458fc8 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2026_medium/config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2026_medium/lines.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2026_medium/lines.png index 5ae1eb82f1..c8af94666c 100644 Binary files a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2026_medium/lines.png and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/german_open_2026_medium/lines.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/config.yaml index 44c36d8201..d5f0908e26 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid.yaml index 1106bab8f2..1026263948 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/hsl_kid.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/lines.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/lines.png index c216972040..cf66cde70c 100644 Binary files a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/lines.png and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/hsl_kid/lines.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/config.yaml index 121f85dafd..47ecd4a93c 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/gui-config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/gui-config.yaml index 92f274ba3c..378247398d 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/gui-config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/lines.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/lines.png index aa9ecf8b86..fd6103fb87 100644 Binary files a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/lines.png and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/ifa26/lines.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/labor/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/labor/config.yaml index 8917ea84d3..d935534f0a 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/labor/config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/labor/lines.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/labor/lines.png index 4afe4ee4e4..f08d39773e 100644 Binary files a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/labor/lines.png and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/labor/lines.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/config.yaml index cb8b9fbfac..b50fc6760e 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/gui-config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/gui-config.yaml index 4af89b055f..103caa73ef 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/gui-config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/lines.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/lines.png index 1003a4c9c2..9e20b9fb2d 100644 Binary files a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/lines.png and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/middle_division_2026/lines.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/config.yaml index c856235872..a1b393d789 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/gui-config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/gui-config.yaml index 05e7a4deaa..b292e3541a 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/gui-config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/lines.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/lines.png index da87659b4d..ef18b2eee5 100644 Binary files a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/lines.png and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/small_division_2026/lines.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/spl/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/spl/config.yaml index b75aa92017..5bac6a362f 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/spl/config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/spl/lines.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/spl/lines.png index f0746b6638..1de7ba13c3 100644 Binary files a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/spl/lines.png and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/spl/lines.png differ diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/webots/config.yaml b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/webots/config.yaml index 1a7458be01..610d0a1d29 100644 --- a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/webots/config.yaml +++ b/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 diff --git a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/webots/lines.png b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/webots/lines.png index 9f8e8ae858..2609bb4d41 100644 Binary files a/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/webots/lines.png and b/src/bitbots_misc/bitbots_parameter_blackboard/config/fields/webots/lines.png differ diff --git a/src/bitbots_navigation/bitbots_localization/CMakeLists.txt b/src/bitbots_navigation/bitbots_localization/CMakeLists.txt index 8e482344af..058a5825dd 100644 --- a/src/bitbots_navigation/bitbots_localization/CMakeLists.txt +++ b/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() diff --git a/src/bitbots_navigation/bitbots_localization/README.md b/src/bitbots_navigation/bitbots_localization/README.md index 73f37f2d64..25deea431c 100644 --- a/src/bitbots_navigation/bitbots_localization/README.md +++ b/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. diff --git a/src/bitbots_navigation/bitbots_localization/src/localization.cpp b/src/bitbots_navigation/bitbots_localization/src/localization.cpp index b173f7a66c..ce1855bc40 100644 --- a/src/bitbots_navigation/bitbots_localization/src/localization.cpp +++ b/src/bitbots_navigation/bitbots_localization/src/localization.cpp @@ -530,7 +530,7 @@ void Localization::publish_debug_rating(const std::vector= 0) { - color.r = occupancy / 100; + color.r = occupancy; } else { color.g = 1; } diff --git a/src/bitbots_navigation/bitbots_localization/src/map.cpp b/src/bitbots_navigation/bitbots_localization/src/map.cpp index 15c301eaf9..a4899bada4 100644 --- a/src/bitbots_navigation/bitbots_localization/src/map.cpp +++ b/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(y, x); + return (255 - map.at(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(i, j); + map_msg.data[i * map.cols + j] = std::lround((255 - map.at(i, j)) * 100.0 / 255.0); } } return map_msg; diff --git a/src/bitbots_navigation/bitbots_localization/test/test_map.cpp b/src/bitbots_navigation/bitbots_localization/test/test_map.cpp new file mode 100644 index 0000000000..cceac4300c --- /dev/null +++ b/src/bitbots_navigation/bitbots_localization/test/test_map.cpp @@ -0,0 +1,27 @@ +#include +#include +#include + +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(0, x) = x; + map.map.at(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; +}