Skip to content

Commit

Permalink
Make ci happy
Browse files Browse the repository at this point in the history
  • Loading branch information
Flova committed Dec 21, 2023
1 parent 2ba4f6e commit 3dd8313
Show file tree
Hide file tree
Showing 4 changed files with 294 additions and 251 deletions.
49 changes: 32 additions & 17 deletions soccer_field_map_generator/soccer_field_map_generator/cli.py
Original file line number Diff line number Diff line change
@@ -1,42 +1,57 @@
import sys
import os
import cv2
import yaml
# Copyright (c) 2023 Hamburg Bit-Bots
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.


import argparse
import os
import sys

import cv2
from soccer_field_map_generator.generator import (
generate_map_image,
generate_metadata,
load_config_file,
)
import yaml


def main():
parser = argparse.ArgumentParser(description="Generate maps for localization")
parser.add_argument("output", help="Output file name")
parser = argparse.ArgumentParser(description='Generate maps for localization')
parser.add_argument('output', help='Output file name')
parser.add_argument(
"config",
help="Config file for the generator that specifies the parameters for the map generation",
'config',
help='Config file for the generator that specifies the parameters for the map generation',
)
parser.add_argument(
"--metadata",
'--metadata',
help="Also generates a 'map_server.yaml' file with the metadata for the map",
action="store_true",
action='store_true',
)
args = parser.parse_args()

# Check if the config file exists
if not os.path.isfile(args.config):
print("Config file does not exist")
print('Config file does not exist')
sys.exit(1)

# Load config file
with open(args.config, "r") as config_file:
with open(args.config, 'r') as config_file:
parameters = load_config_file(config_file)

# Check if the config file is valid
if parameters is None:
print("Invalid config file")
print('Invalid config file')
sys.exit(1)

# Generate the map image
Expand All @@ -47,7 +62,7 @@ def main():

# Check if the output folder exists
if not os.path.isdir(os.path.dirname(output_path)):
print("Output folder does not exist")
print('Output folder does not exist')
sys.exit(1)

# Save the image
Expand All @@ -57,11 +72,11 @@ def main():
if args.metadata:
metadata = generate_metadata(parameters, os.path.basename(output_path))
metadata_file_name = os.path.join(
os.path.dirname(output_path), "map_server.yaml"
os.path.dirname(output_path), 'map_server.yaml'
)
with open(metadata_file_name, "w") as metadata_file:
with open(metadata_file_name, 'w') as metadata_file:
yaml.dump(metadata, metadata_file, sort_keys=False)


if __name__ == "__main__":
if __name__ == '__main__':
main()
125 changes: 69 additions & 56 deletions soccer_field_map_generator/soccer_field_map_generator/generator.py
Original file line number Diff line number Diff line change
@@ -1,33 +1,46 @@
#!/usr/bin/env python3
# Copyright (c) 2023 Hamburg Bit-Bots
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.


from enum import Enum
import math
import yaml
from typing import Optional

import cv2
import numpy as np
from typing import Optional
from scipy import ndimage
from enum import Enum
import yaml


class MapTypes(Enum):
LINE = "line"
POSTS = "posts"
FIELD_BOUNDARY = "field_boundary"
FIELD_FEATURES = "field_features"
CORNERS = "corners"
TCROSSINGS = "tcrossings"
CROSSES = "crosses"
LINE = 'line'
POSTS = 'posts'
FIELD_BOUNDARY = 'field_boundary'
FIELD_FEATURES = 'field_features'
CORNERS = 'corners'
TCROSSINGS = 'tcrossings'
CROSSES = 'crosses'


class MarkTypes(Enum):
POINT = "point"
CROSS = "cross"
POINT = 'point'
CROSS = 'cross'


class FieldFeatureStyles(Enum):
EXACT = "exact"
BLOB = "blob"
EXACT = 'exact'
BLOB = 'blob'


def drawCross(img, point, color, width=5, length=15):
Expand Down Expand Up @@ -62,30 +75,30 @@ def drawDistance(image, decay_factor):


def generate_map_image(parameters):
target = MapTypes(parameters["map_type"])
mark_type = MarkTypes(parameters["mark_type"])
field_feature_style = FieldFeatureStyles(parameters["field_feature_style"])

penalty_mark = parameters["penalty_mark"]
center_point = parameters["center_point"]
goal_back = parameters["goal_back"] # Draw goal back area

stroke_width = parameters["stroke_width"]
field_length = parameters["field_length"]
field_width = parameters["field_width"]
goal_depth = parameters["goal_depth"]
goal_width = parameters["goal_width"]
goal_area_length = parameters["goal_area_length"]
goal_area_width = parameters["goal_area_width"]
penalty_mark_distance = parameters["penalty_mark_distance"]
center_circle_diameter = parameters["center_circle_diameter"]
border_strip_width = parameters["border_strip_width"]
penalty_area_length = parameters["penalty_area_length"]
penalty_area_width = parameters["penalty_area_width"]
field_feature_size = parameters["field_feature_size"]
distance_map = parameters["distance_map"]
distance_decay = parameters["distance_decay"]
invert = parameters["invert"]
target = MapTypes(parameters['map_type'])
mark_type = MarkTypes(parameters['mark_type'])
field_feature_style = FieldFeatureStyles(parameters['field_feature_style'])

penalty_mark = parameters['penalty_mark']
center_point = parameters['center_point']
goal_back = parameters['goal_back'] # Draw goal back area

stroke_width = parameters['stroke_width']
field_length = parameters['field_length']
field_width = parameters['field_width']
goal_depth = parameters['goal_depth']
goal_width = parameters['goal_width']
goal_area_length = parameters['goal_area_length']
goal_area_width = parameters['goal_area_width']
penalty_mark_distance = parameters['penalty_mark_distance']
center_circle_diameter = parameters['center_circle_diameter']
border_strip_width = parameters['border_strip_width']
penalty_area_length = parameters['penalty_area_length']
penalty_area_width = parameters['penalty_area_width']
field_feature_size = parameters['field_feature_size']
distance_map = parameters['distance_map']
distance_decay = parameters['distance_decay']
invert = parameters['invert']

# Color of the lines and marks
color = (255, 255, 255) # white
Expand Down Expand Up @@ -197,7 +210,7 @@ def generate_map_image(parameters):
elif mark_type == MarkTypes.CROSS:
drawCross(img, middle_point, color, stroke_width)
else:
raise NotImplementedError("Mark type not implemented")
raise NotImplementedError('Mark type not implemented')

# Draw penalty marks (point or cross)
if penalty_mark:
Expand All @@ -208,7 +221,7 @@ def generate_map_image(parameters):
drawCross(img, penalty_mark_left, color, stroke_width)
drawCross(img, penalty_mark_right, color, stroke_width)
else:
raise NotImplementedError("Mark type not implemented")
raise NotImplementedError('Mark type not implemented')

# Draw goal area
img = cv2.rectangle(
Expand Down Expand Up @@ -657,23 +670,23 @@ def generate_map_image(parameters):
def generate_metadata(parameters: dict, image_name: str) -> dict:
# Get the field dimensions in cm
field_dimensions = np.array(
[parameters["field_length"], parameters["field_width"], 0]
[parameters['field_length'], parameters['field_width'], 0]
)
# Add the border strip
field_dimensions[:2] += 2 * parameters["border_strip_width"]
field_dimensions[:2] += 2 * parameters['border_strip_width']
# Get the origin
origin = -field_dimensions / 2
# Convert to meters
origin /= 100

# Generate the metadata
return {
"image": image_name,
"resolution": 0.01,
"origin": origin.tolist(),
"occupied_thresh": 0.99,
"free_thresh": 0.196,
"negate": int(parameters["invert"]),
'image': image_name,
'resolution': 0.01,
'origin': origin.tolist(),
'occupied_thresh': 0.99,
'free_thresh': 0.196,
'negate': int(parameters['invert']),
}


Expand All @@ -682,11 +695,11 @@ def load_config_file(file) -> Optional[dict]:
config_file = yaml.load(file, Loader=yaml.FullLoader)
# Check if the file is valid (has the correct fields)
if (
"header" in config_file
and "type" in config_file["header"]
and "version" in config_file["header"]
and "parameters" in config_file
and config_file["header"]["version"] == "1.0"
and config_file["header"]["type"] == "map_generator_config"
'header' in config_file
and 'type' in config_file['header']
and 'version' in config_file['header']
and 'parameters' in config_file
and config_file['header']['version'] == '1.0'
and config_file['header']['type'] == 'map_generator_config'
):
return config_file["parameters"]
return config_file['parameters']
Loading

0 comments on commit 3dd8313

Please sign in to comment.