diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..59acd29
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,5 @@
+__pycache__/
+.vscode/*
+log/*
+
+*.pyc
\ No newline at end of file
diff --git a/README.md b/README.md
index c64ee00..3b79b3c 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,45 @@
-# Template Repo
+# Soccer Field Map Generator
[](../../actions/workflows/build_and_test_humble.yaml?query=branch:rolling)
[](../../actions/workflows/build_and_test_iron.yaml?query=branch:rolling)
[](../../actions/workflows/build_and_test_rolling.yaml?query=branch:rolling)
+
+This repository contains a tool for generating soccer field maps. It includes a GUI for interactively creating and editing maps, as well as a command-line interface for scripted map generation.
+
+## Installation
+
+To install the tool, run the following commands in your colcon workspace:
+
+```bash
+git clone git@github.com:ros-sports/soccer_field_map_generator.git src/soccer_field_map_generator
+rosdep install --from-paths src --ignore-src -r -y
+colcon build
+```
+
+Don't forget to source your workspace after building:
+
+```bash
+source install/setup.bash
+```
+
+## Usage
+
+### GUI
+
+To launch the GUI, run the following command:
+
+```bash
+ros2 run soccer_field_map_generator gui
+```
+
+You should see a window like this:
+
+
+
+### CLI
+
+To generate a map using the command-line interface, run the following command:
+
+```bash
+ros2 run soccer_field_map_generator cli [output_file] [config_file] [options]
+```
diff --git a/gui.png b/gui.png
new file mode 100644
index 0000000..634370d
Binary files /dev/null and b/gui.png differ
diff --git a/soccer_field_map_generator/package.xml b/soccer_field_map_generator/package.xml
new file mode 100644
index 0000000..c513a91
--- /dev/null
+++ b/soccer_field_map_generator/package.xml
@@ -0,0 +1,25 @@
+
+
+
+ soccer_field_map_generator
+ 0.0.0
+ A package to generate a soccer field map using a gui or cli
+ Florian Vahl
+ Apache License 2.0
+
+ python3-opencv
+ python3-numpy
+ python3-tk
+ python3-yaml
+ python3-pil
+ python3-scipy
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/soccer_field_map_generator/resource/soccer_field_map_generator b/soccer_field_map_generator/resource/soccer_field_map_generator
new file mode 100644
index 0000000..e69de29
diff --git a/soccer_field_map_generator/setup.cfg b/soccer_field_map_generator/setup.cfg
new file mode 100644
index 0000000..4ff91cd
--- /dev/null
+++ b/soccer_field_map_generator/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/soccer_field_map_generator
+[install]
+install_scripts=$base/lib/soccer_field_map_generator
diff --git a/soccer_field_map_generator/setup.py b/soccer_field_map_generator/setup.py
new file mode 100644
index 0000000..58f0679
--- /dev/null
+++ b/soccer_field_map_generator/setup.py
@@ -0,0 +1,27 @@
+from setuptools import find_packages, setup
+
+package_name = 'soccer_field_map_generator'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='florian',
+ maintainer_email='git@flova.de',
+ description='A package to generate a soccer field map using a gui or cli',
+ license='Apache License 2.0',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'gui = soccer_field_map_generator.gui:main',
+ 'cli = soccer_field_map_generator.cli:main',
+ ],
+ },
+)
diff --git a/soccer_field_map_generator/soccer_field_map_generator/__init__.py b/soccer_field_map_generator/soccer_field_map_generator/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/soccer_field_map_generator/soccer_field_map_generator/cli.py b/soccer_field_map_generator/soccer_field_map_generator/cli.py
new file mode 100644
index 0000000..161a0be
--- /dev/null
+++ b/soccer_field_map_generator/soccer_field_map_generator/cli.py
@@ -0,0 +1,82 @@
+# 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.add_argument(
+ 'config',
+ help='Config file for the generator that specifies the parameters for the map generation',
+ )
+ parser.add_argument(
+ '--metadata',
+ help="Also generates a 'map_server.yaml' file with the metadata for the map",
+ 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')
+ sys.exit(1)
+
+ # Load 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')
+ sys.exit(1)
+
+ # Generate the map image
+ image = generate_map_image(parameters)
+
+ # Make output folder full path
+ output_path = os.path.abspath(args.output)
+
+ # Check if the output folder exists
+ if not os.path.isdir(os.path.dirname(output_path)):
+ print('Output folder does not exist')
+ sys.exit(1)
+
+ # Save the image
+ cv2.imwrite(output_path, image)
+
+ # Generate the metadata
+ 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'
+ )
+ with open(metadata_file_name, 'w') as metadata_file:
+ yaml.dump(metadata, metadata_file, sort_keys=False)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/soccer_field_map_generator/soccer_field_map_generator/generator.py b/soccer_field_map_generator/soccer_field_map_generator/generator.py
new file mode 100755
index 0000000..2bcb768
--- /dev/null
+++ b/soccer_field_map_generator/soccer_field_map_generator/generator.py
@@ -0,0 +1,705 @@
+# 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
+from typing import Optional
+
+import cv2
+import numpy as np
+from scipy import ndimage
+import yaml
+
+
+class MapTypes(Enum):
+ 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'
+
+
+class FieldFeatureStyles(Enum):
+ EXACT = 'exact'
+ BLOB = 'blob'
+
+
+def drawCross(img, point, color, width=5, length=15):
+ vertical_start = (point[0], point[1] - length)
+ vertical_end = (point[0], point[1] + length)
+ horizontal_start = (point[0] - length, point[1])
+ horizontal_end = (point[0] + length, point[1])
+ img = cv2.line(img, vertical_start, vertical_end, color, width)
+ img = cv2.line(img, horizontal_start, horizontal_end, color, width)
+
+
+def drawDistance(image, decay_factor):
+ # Calc distances
+ distance_map = ndimage.distance_transform_edt(255 - image)
+
+ # Activation function that leads to a faster decay at the beginning
+ # and a slower decay at the end and is parameterized by the decay factor
+ if not math.isclose(decay_factor, 0):
+ distance_map = np.exp(-distance_map * decay_factor * 0.1)
+ else:
+ distance_map = -distance_map
+
+ # Scale to bounds of uint8 (0-255)
+ return cv2.normalize(
+ distance_map,
+ None,
+ alpha=0,
+ beta=255,
+ norm_type=cv2.NORM_MINMAX,
+ dtype=cv2.CV_64F,
+ ).astype(np.uint8)
+
+
+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']
+
+ # Color of the lines and marks
+ color = (255, 255, 255) # white
+
+ # Size of complete turf field (field with outside borders)
+ image_size = (
+ field_width + border_strip_width * 2,
+ field_length + border_strip_width * 2,
+ 3,
+ )
+
+ # Calculate important points on the field
+ field_outline_start = (border_strip_width, border_strip_width)
+ field_outline_end = (
+ field_length + border_strip_width,
+ field_width + border_strip_width,
+ )
+
+ middle_line_start = (field_length // 2 + border_strip_width, border_strip_width)
+ middle_line_end = (
+ field_length // 2 + border_strip_width,
+ field_width + border_strip_width,
+ )
+
+ middle_point = (
+ field_length // 2 + border_strip_width,
+ field_width // 2 + border_strip_width,
+ )
+
+ penalty_mark_left = (
+ penalty_mark_distance + border_strip_width,
+ field_width // 2 + border_strip_width,
+ )
+ penalty_mark_right = (
+ image_size[1] - border_strip_width - penalty_mark_distance,
+ field_width // 2 + border_strip_width,
+ )
+
+ goal_area_left_start = (
+ border_strip_width,
+ border_strip_width + field_width // 2 - goal_area_width // 2,
+ )
+ goal_area_left_end = (
+ border_strip_width + goal_area_length,
+ field_width // 2 + border_strip_width + goal_area_width // 2,
+ )
+
+ goal_area_right_start = (
+ image_size[1] - goal_area_left_start[0],
+ goal_area_left_start[1],
+ )
+ goal_area_right_end = (image_size[1] - goal_area_left_end[0], goal_area_left_end[1])
+
+ penalty_area_left_start = (
+ border_strip_width,
+ border_strip_width + field_width // 2 - penalty_area_width // 2,
+ )
+ penalty_area_left_end = (
+ border_strip_width + penalty_area_length,
+ field_width // 2 + border_strip_width + penalty_area_width // 2,
+ )
+
+ penalty_area_right_start = (
+ image_size[1] - penalty_area_left_start[0],
+ penalty_area_left_start[1],
+ )
+ penalty_area_right_end = (
+ image_size[1] - penalty_area_left_end[0],
+ penalty_area_left_end[1],
+ )
+
+ goalpost_left_1 = (
+ border_strip_width,
+ border_strip_width + field_width // 2 + goal_width // 2,
+ )
+ goalpost_left_2 = (
+ border_strip_width,
+ border_strip_width + field_width // 2 - goal_width // 2,
+ )
+
+ goalpost_right_1 = (image_size[1] - goalpost_left_1[0], goalpost_left_1[1])
+ goalpost_right_2 = (image_size[1] - goalpost_left_2[0], goalpost_left_2[1])
+
+ goal_back_corner_left = (goalpost_left_2[0] - goal_depth, goalpost_left_2[1])
+ goal_back_corner_right = (goalpost_right_2[0] + goal_depth, goalpost_right_2[1])
+
+ # Create black image in the correct size
+ img = np.zeros(image_size, np.uint8)
+
+ # Check which map type we want to generate
+ if target == MapTypes.LINE:
+ # Draw outline
+ img = cv2.rectangle(
+ img, field_outline_start, field_outline_end, color, stroke_width
+ )
+
+ # Draw middle line
+ img = cv2.line(img, middle_line_start, middle_line_end, color, stroke_width)
+
+ # Draw center circle
+ img = cv2.circle(
+ img, middle_point, center_circle_diameter // 2, color, stroke_width
+ )
+
+ # Draw center mark (point or cross)
+ if center_point:
+ if mark_type == MarkTypes.POINT:
+ img = cv2.circle(img, middle_point, stroke_width * 2, color, -1)
+ elif mark_type == MarkTypes.CROSS:
+ drawCross(img, middle_point, color, stroke_width)
+ else:
+ raise NotImplementedError('Mark type not implemented')
+
+ # Draw penalty marks (point or cross)
+ if penalty_mark:
+ if mark_type == MarkTypes.POINT:
+ img = cv2.circle(img, penalty_mark_left, stroke_width * 2, color, -1)
+ img = cv2.circle(img, penalty_mark_right, stroke_width * 2, color, -1)
+ elif mark_type == MarkTypes.CROSS:
+ drawCross(img, penalty_mark_left, color, stroke_width)
+ drawCross(img, penalty_mark_right, color, stroke_width)
+ else:
+ raise NotImplementedError('Mark type not implemented')
+
+ # Draw goal area
+ img = cv2.rectangle(
+ img, goal_area_left_start, goal_area_left_end, color, stroke_width
+ )
+ img = cv2.rectangle(
+ img, goal_area_right_start, goal_area_right_end, color, stroke_width
+ )
+
+ # Draw penalty area
+ img = cv2.rectangle(
+ img, penalty_area_left_start, penalty_area_left_end, color, stroke_width
+ )
+ img = cv2.rectangle(
+ img, penalty_area_right_start, penalty_area_right_end, color, stroke_width
+ )
+
+ # Draw goal back area
+ if goal_back:
+ img = cv2.rectangle(
+ img, goalpost_left_1, goal_back_corner_left, color, stroke_width
+ )
+ img = cv2.rectangle(
+ img, goalpost_right_1, goal_back_corner_right, color, stroke_width
+ )
+
+ if target == MapTypes.POSTS:
+ # Draw goalposts
+ img = cv2.circle(img, goalpost_left_1, stroke_width * 2, color, -1)
+ img = cv2.circle(img, goalpost_left_2, stroke_width * 2, color, -1)
+ img = cv2.circle(img, goalpost_right_1, stroke_width * 2, color, -1)
+ img = cv2.circle(img, goalpost_right_2, stroke_width * 2, color, -1)
+
+ if target == MapTypes.FIELD_BOUNDARY:
+ # We need a larger image for this as we draw outside the field
+ img = np.zeros((image_size[0] + 200, image_size[1] + 200), np.uint8)
+
+ # Draw fieldboundary
+ img = cv2.rectangle(
+ img,
+ (100, 100),
+ (image_size[1] + 100, image_size[0] + 100),
+ color,
+ stroke_width,
+ )
+
+ if (
+ target in [MapTypes.CORNERS, MapTypes.FIELD_FEATURES]
+ and field_feature_style == FieldFeatureStyles.EXACT
+ ):
+ # draw outline corners
+ # top left
+ img = cv2.line(
+ img,
+ field_outline_start,
+ (field_outline_start[0], field_outline_start[1] + field_feature_size),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ field_outline_start,
+ (field_outline_start[0] + field_feature_size, field_outline_start[1]),
+ color,
+ stroke_width,
+ )
+
+ # bottom left
+ img = cv2.line(
+ img,
+ (field_outline_start[0], field_outline_end[1]),
+ (field_outline_start[0], field_outline_end[1] - field_feature_size),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ (field_outline_start[0], field_outline_end[1]),
+ (field_outline_start[0] + field_feature_size, field_outline_end[1]),
+ color,
+ stroke_width,
+ )
+
+ # top right
+ img = cv2.line(
+ img,
+ (field_outline_end[0], field_outline_start[1]),
+ (field_outline_end[0], field_outline_start[1] + field_feature_size),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ (field_outline_end[0], field_outline_start[1]),
+ (field_outline_end[0] - field_feature_size, field_outline_start[1]),
+ color,
+ stroke_width,
+ )
+
+ # bottom right
+ img = cv2.line(
+ img,
+ field_outline_end,
+ (field_outline_end[0], field_outline_end[1] - field_feature_size),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ field_outline_end,
+ (field_outline_end[0] - field_feature_size, field_outline_end[1]),
+ color,
+ stroke_width,
+ )
+
+ # draw left goal area corners
+ # top
+ img = cv2.line(
+ img,
+ (goal_area_left_end[0], goal_area_left_start[1]),
+ (goal_area_left_end[0], goal_area_left_start[1] + field_feature_size),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ (goal_area_left_end[0], goal_area_left_start[1]),
+ (goal_area_left_end[0] - field_feature_size, goal_area_left_start[1]),
+ color,
+ stroke_width,
+ )
+
+ # bottom
+
+ img = cv2.line(
+ img,
+ goal_area_left_end,
+ (goal_area_left_end[0], goal_area_left_end[1] - field_feature_size),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ goal_area_left_end,
+ (goal_area_left_end[0] - field_feature_size, goal_area_left_end[1]),
+ color,
+ stroke_width,
+ )
+
+ # draw right goal aera corners
+
+ # top
+
+ img = cv2.line(
+ img,
+ (goal_area_right_end[0], goal_area_right_start[1]),
+ (goal_area_right_end[0], goal_area_right_start[1] + field_feature_size),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ (goal_area_right_end[0], goal_area_right_start[1]),
+ (goal_area_right_end[0] + field_feature_size, goal_area_right_start[1]),
+ color,
+ stroke_width,
+ )
+
+ # bottom
+
+ img = cv2.line(
+ img,
+ goal_area_right_end,
+ (goal_area_right_end[0], goal_area_right_end[1] - field_feature_size),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ goal_area_right_end,
+ (goal_area_right_end[0] + field_feature_size, goal_area_right_end[1]),
+ color,
+ stroke_width,
+ )
+
+ if (
+ target in [MapTypes.CORNERS, MapTypes.FIELD_FEATURES]
+ and field_feature_style == FieldFeatureStyles.BLOB
+ ):
+ # field corners
+ img = cv2.circle(img, field_outline_start, field_feature_size, color, -1)
+ img = cv2.circle(
+ img,
+ (field_outline_start[0], field_outline_end[1]),
+ field_feature_size,
+ color,
+ -1,
+ )
+ img = cv2.circle(
+ img,
+ (field_outline_end[0], field_outline_start[1]),
+ field_feature_size,
+ color,
+ -1,
+ )
+ img = cv2.circle(img, field_outline_end, field_feature_size, color, -1)
+
+ # goal area corners
+ img = cv2.circle(
+ img,
+ (goal_area_left_end[0], goal_area_left_start[1]),
+ field_feature_size,
+ color,
+ -1,
+ )
+ img = cv2.circle(img, goal_area_left_end, field_feature_size, color, -1)
+ img = cv2.circle(
+ img,
+ (goal_area_right_end[0], goal_area_right_start[1]),
+ field_feature_size,
+ color,
+ -1,
+ )
+ img = cv2.circle(img, goal_area_right_end, field_feature_size, color, -1)
+
+ if (
+ target in [MapTypes.TCROSSINGS, MapTypes.FIELD_FEATURES]
+ and field_feature_style == FieldFeatureStyles.EXACT
+ ):
+ # draw left goal area
+ # top
+ img = cv2.line(
+ img,
+ (
+ goal_area_left_start[0],
+ goal_area_left_start[1] - field_feature_size // 2,
+ ),
+ (
+ goal_area_left_start[0],
+ goal_area_left_start[1] + field_feature_size // 2,
+ ),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ goal_area_left_start,
+ (goal_area_left_start[0] + field_feature_size, goal_area_left_start[1]),
+ color,
+ stroke_width,
+ )
+
+ # bottom
+ img = cv2.line(
+ img,
+ (goal_area_left_start[0], goal_area_left_end[1] - field_feature_size // 2),
+ (goal_area_left_start[0], goal_area_left_end[1] + field_feature_size // 2),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ (goal_area_left_start[0], goal_area_left_end[1]),
+ (goal_area_left_start[0] + field_feature_size, goal_area_left_end[1]),
+ color,
+ stroke_width,
+ )
+ # draw right goal area
+
+ # top
+ img = cv2.line(
+ img,
+ (
+ goal_area_right_start[0],
+ goal_area_right_start[1] - field_feature_size // 2,
+ ),
+ (
+ goal_area_right_start[0],
+ goal_area_right_start[1] + field_feature_size // 2,
+ ),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ goal_area_right_start,
+ (goal_area_right_start[0] - field_feature_size, goal_area_right_start[1]),
+ color,
+ stroke_width,
+ )
+
+ # bottom
+ img = cv2.line(
+ img,
+ (
+ goal_area_right_start[0],
+ goal_area_right_end[1] - field_feature_size // 2,
+ ),
+ (
+ goal_area_right_start[0],
+ goal_area_right_end[1] + field_feature_size // 2,
+ ),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ (goal_area_right_start[0], goal_area_right_end[1]),
+ (goal_area_right_start[0] - field_feature_size, goal_area_right_end[1]),
+ color,
+ stroke_width,
+ )
+
+ # draw center line to side line t crossings
+ # top
+ img = cv2.line(
+ img,
+ (middle_line_start[0] - field_feature_size // 2, middle_line_start[1]),
+ (middle_line_start[0] + field_feature_size // 2, middle_line_start[1]),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ middle_line_start,
+ (middle_line_start[0], middle_line_start[1] + field_feature_size),
+ color,
+ stroke_width,
+ )
+
+ # bottom
+ img = cv2.line(
+ img,
+ (middle_line_end[0] - field_feature_size // 2, middle_line_end[1]),
+ (middle_line_end[0] + field_feature_size // 2, middle_line_end[1]),
+ color,
+ stroke_width,
+ )
+ img = cv2.line(
+ img,
+ middle_line_end,
+ (middle_line_end[0], middle_line_end[1] - field_feature_size),
+ color,
+ stroke_width,
+ )
+
+ if (
+ target in [MapTypes.TCROSSINGS, MapTypes.FIELD_FEATURES]
+ and field_feature_style == FieldFeatureStyles.BLOB
+ ):
+ # draw blobs for goal areas
+ img = cv2.circle(img, goal_area_left_start, field_feature_size, color, -1)
+ img = cv2.circle(
+ img,
+ (goal_area_left_start[0], goal_area_left_end[1]),
+ field_feature_size,
+ color,
+ -1,
+ )
+ img = cv2.circle(img, goal_area_right_start, field_feature_size, color, -1)
+ img = cv2.circle(
+ img,
+ (goal_area_right_start[0], goal_area_right_end[1]),
+ field_feature_size,
+ color,
+ -1,
+ )
+
+ # middle line
+ img = cv2.circle(img, middle_line_start, field_feature_size, color, -1)
+ img = cv2.circle(img, middle_line_end, field_feature_size, color, -1)
+
+ if (
+ target in [MapTypes.CROSSES, MapTypes.FIELD_FEATURES]
+ and field_feature_style == FieldFeatureStyles.EXACT
+ ):
+ # penalty marks
+ if penalty_mark:
+ drawCross(
+ img, penalty_mark_left, color, stroke_width, field_feature_size // 2
+ )
+ drawCross(
+ img, penalty_mark_right, color, stroke_width, field_feature_size // 2
+ )
+
+ # middle point
+ if center_point:
+ drawCross(img, middle_point, color, stroke_width, field_feature_size // 2)
+
+ # center circle middle line crossings
+ drawCross(
+ img,
+ (middle_point[0], middle_point[1] - center_circle_diameter),
+ color,
+ stroke_width,
+ field_feature_size // 2,
+ )
+ drawCross(
+ img,
+ (middle_point[0], middle_point[1] + center_circle_diameter),
+ color,
+ stroke_width,
+ field_feature_size // 2,
+ )
+
+ if (
+ target in [MapTypes.CROSSES, MapTypes.FIELD_FEATURES]
+ and field_feature_style == FieldFeatureStyles.BLOB
+ ):
+ # penalty marks
+ if penalty_mark:
+ img = cv2.circle(img, penalty_mark_left, field_feature_size, color, -1)
+ img = cv2.circle(img, penalty_mark_right, field_feature_size, color, -1)
+
+ # middle point
+ if center_point:
+ img = cv2.circle(img, middle_point, field_feature_size, color, -1)
+
+ # center circle middle line crossings
+ img = cv2.circle(
+ img,
+ (middle_point[0], middle_point[1] - center_circle_diameter),
+ field_feature_size,
+ color,
+ -1,
+ )
+ img = cv2.circle(
+ img,
+ (middle_point[0], middle_point[1] + center_circle_diameter),
+ field_feature_size,
+ color,
+ -1,
+ )
+
+ # Create distance map
+ if distance_map:
+ img = drawDistance(img, distance_decay)
+
+ # Invert
+ if invert:
+ img = 255 - img
+
+ return img
+
+
+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]
+ )
+ # Add the border strip
+ 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']),
+ }
+
+
+def load_config_file(file) -> Optional[dict]:
+ # Load the parameters from the file
+ 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'
+ ):
+ return config_file['parameters']
diff --git a/soccer_field_map_generator/soccer_field_map_generator/gui.py b/soccer_field_map_generator/soccer_field_map_generator/gui.py
new file mode 100644
index 0000000..6565020
--- /dev/null
+++ b/soccer_field_map_generator/soccer_field_map_generator/gui.py
@@ -0,0 +1,424 @@
+# 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 os
+import tkinter as tk
+from tkinter import filedialog
+from tkinter import ttk
+
+import cv2
+from PIL import Image, ImageTk
+from soccer_field_map_generator.generator import (
+ FieldFeatureStyles,
+ generate_map_image,
+ generate_metadata,
+ load_config_file,
+ MapTypes,
+ MarkTypes,
+)
+from soccer_field_map_generator.tooltip import Tooltip
+import yaml
+
+
+class MapGeneratorParamInput(tk.Frame):
+
+ def __init__(
+ self, parent, update_hook: callable, parameter_definitions: dict[str, dict]
+ ):
+ tk.Frame.__init__(self, parent)
+
+ # Keep track of parameter definitions, GUI elements and the input values
+ self.parameter_definitions = parameter_definitions
+ self.parameter_ui_elements: dict[str, dict[str, ttk.Widget]] = {}
+ self.parameter_values: dict[str, tk.Variable] = {}
+
+ # Generate GUI elements for all parameters
+ for parameter_name, parameter_definition in parameter_definitions.items():
+ # Create GUI elements
+ label = ttk.Label(self, text=parameter_definition['label'])
+ if parameter_definition['type'] == bool:
+ variable = tk.BooleanVar(value=parameter_definition['default'])
+ ui_element = ttk.Checkbutton(
+ self, command=update_hook, variable=variable
+ )
+ elif parameter_definition['type'] == int:
+ variable = tk.IntVar(value=parameter_definition['default'])
+ ui_element = ttk.Entry(self, textvariable=variable)
+ ui_element.bind('', update_hook)
+ elif parameter_definition['type'] == float:
+ variable = tk.DoubleVar(value=parameter_definition['default'])
+ ui_element = ttk.Entry(self, textvariable=variable)
+ ui_element.bind('', update_hook)
+ elif issubclass(parameter_definition['type'], Enum):
+ variable = tk.StringVar(value=parameter_definition['default'].value)
+ values = [enum.value for enum in parameter_definition['type']]
+ ui_element = ttk.Combobox(self, values=values, textvariable=variable)
+ ui_element.bind('<>', update_hook)
+ else:
+ raise NotImplementedError('Parameter type not implemented')
+
+ # Add tooltips
+ Tooltip(label, text=parameter_definition['tooltip'])
+ Tooltip(ui_element, text=parameter_definition['tooltip'])
+
+ # Add ui elements to the dict
+ self.parameter_ui_elements[parameter_name] = {
+ 'label': label,
+ 'ui_element': ui_element,
+ }
+
+ # Store variable for later state access
+ self.parameter_values[parameter_name] = variable
+
+ # Create layout
+ for i, parameter_name in enumerate(parameter_definitions.keys()):
+ self.parameter_ui_elements[parameter_name]['label'].grid(
+ row=i, column=0, sticky='e'
+ )
+ self.parameter_ui_elements[parameter_name]['ui_element'].grid(
+ row=i, column=1, sticky='w'
+ )
+
+ def get_parameters(self):
+ return {
+ parameter_name: parameter_value.get()
+ for parameter_name, parameter_value in self.parameter_values.items()
+ }
+
+ def get_parameter(self, parameter_name):
+ return self.parameter_values[parameter_name].get()
+
+
+class MapGeneratorGUI:
+
+ def __init__(self, root: tk.Tk):
+ # Set ttk theme
+ s = ttk.Style()
+ s.theme_use('clam')
+
+ # Set window title and size
+ self.root = root
+ self.root.title('Map Generator GUI')
+ self.root.resizable(False, False)
+
+ # Create GUI elements
+
+ # Title
+ self.title = ttk.Label(
+ self.root, text='Soccer Map Generator', font=('TkDefaultFont', 16)
+ )
+
+ # Parameter Input
+ self.parameter_input = MapGeneratorParamInput(
+ self.root,
+ self.update_map,
+ {
+ 'map_type': {
+ 'type': MapTypes,
+ 'default': MapTypes.LINE,
+ 'label': 'Map Type',
+ 'tooltip': 'Type of the map we want to generate',
+ },
+ 'penalty_mark': {
+ 'type': bool,
+ 'default': True,
+ 'label': 'Penalty Mark',
+ 'tooltip': 'Whether or not to draw the penalty mark',
+ },
+ 'center_point': {
+ 'type': bool,
+ 'default': True,
+ 'label': 'Center Point',
+ 'tooltip': 'Whether or not to draw the center point',
+ },
+ 'goal_back': {
+ 'type': bool,
+ 'default': True,
+ 'label': 'Goal Back',
+ 'tooltip': 'Whether or not to draw the back area of the goal',
+ },
+ 'stroke_width': {
+ 'type': int,
+ 'default': 5,
+ 'label': 'Stoke Width',
+ 'tooltip': 'Width (in px) of the shapes we draw',
+ },
+ 'field_length': {
+ 'type': int,
+ 'default': 900,
+ 'label': 'Field Length',
+ 'tooltip': 'Length of the field in cm',
+ },
+ 'field_width': {
+ 'type': int,
+ 'default': 600,
+ 'label': 'Field Width',
+ 'tooltip': 'Width of the field in cm',
+ },
+ 'goal_depth': {
+ 'type': int,
+ 'default': 60,
+ 'label': 'Goal Depth',
+ 'tooltip': 'Depth of the goal in cm',
+ },
+ 'goal_width': {
+ 'type': int,
+ 'default': 260,
+ 'label': 'Goal Width',
+ 'tooltip': 'Width of the goal in cm',
+ },
+ 'goal_area_length': {
+ 'type': int,
+ 'default': 100,
+ 'label': 'Goal Area Length',
+ 'tooltip': 'Length of the goal area in cm',
+ },
+ 'goal_area_width': {
+ 'type': int,
+ 'default': 300,
+ 'label': 'Goal Area Width',
+ 'tooltip': 'Width of the goal area in cm',
+ },
+ 'penalty_mark_distance': {
+ 'type': int,
+ 'default': 150,
+ 'label': 'Penalty Mark Distance',
+ 'tooltip': 'Distance of the penalty mark from the goal line in cm',
+ },
+ 'center_circle_diameter': {
+ 'type': int,
+ 'default': 150,
+ 'label': 'Center Circle Diameter',
+ 'tooltip': 'Diameter of the center circle in cm',
+ },
+ 'border_strip_width': {
+ 'type': int,
+ 'default': 100,
+ 'label': 'Border Strip Width',
+ 'tooltip': 'Width of the border strip around the field in cm',
+ },
+ 'penalty_area_length': {
+ 'type': int,
+ 'default': 200,
+ 'label': 'Penalty Area Length',
+ 'tooltip': 'Length of the penalty area in cm',
+ },
+ 'penalty_area_width': {
+ 'type': int,
+ 'default': 500,
+ 'label': 'Penalty Area Width',
+ 'tooltip': 'Width of the penalty area in cm',
+ },
+ 'field_feature_size': {
+ 'type': int,
+ 'default': 30,
+ 'label': 'Field Feature Size',
+ 'tooltip': 'Size of the field features in cm',
+ },
+ 'mark_type': {
+ 'type': MarkTypes,
+ 'default': MarkTypes.CROSS,
+ 'label': 'Mark Type',
+ 'tooltip': 'Type of the marks (penalty mark, center point)',
+ },
+ 'field_feature_style': {
+ 'type': FieldFeatureStyles,
+ 'default': FieldFeatureStyles.EXACT,
+ 'label': 'Field Feature Style',
+ 'tooltip': 'Style of the field features',
+ },
+ 'distance_map': {
+ 'type': bool,
+ 'default': False,
+ 'label': 'Distance Map',
+ 'tooltip': 'Whether or not to draw the distance map',
+ },
+ 'distance_decay': {
+ 'type': float,
+ 'default': 0.0,
+ 'label': 'Distance Decay',
+ 'tooltip': 'Exponential decay applied to the distance map',
+ },
+ 'invert': {
+ 'type': bool,
+ 'default': True,
+ 'label': 'Invert',
+ 'tooltip': 'Invert the final image',
+ },
+ },
+ )
+
+ # Generate Map Button
+ self.save_map_button = ttk.Button(
+ self.root, text='Save Map', command=self.save_map
+ )
+
+ # Save metadata checkbox
+ self.save_metadata = tk.BooleanVar(value=True)
+ self.save_metadata_checkbox = ttk.Checkbutton(
+ self.root, text='Save Metadata', variable=self.save_metadata
+ )
+
+ # Load and save config buttons
+ self.load_config_button = ttk.Button(
+ self.root, text='Load Config', command=self.load_config
+ )
+ self.save_config_button = ttk.Button(
+ self.root, text='Save Config', command=self.save_config
+ )
+
+ # Canvas to display the generated map
+ self.canvas = tk.Canvas(self.root, width=800, height=600)
+
+ # Layout
+
+ # Parameter input and generate button
+ self.title.grid(row=0, column=0, columnspan=2, pady=20, padx=10)
+ self.parameter_input.grid(row=1, column=0, columnspan=2, pady=10, padx=10)
+ self.load_config_button.grid(row=2, column=0, columnspan=1, pady=10)
+ self.save_config_button.grid(row=2, column=1, columnspan=1, pady=10)
+ self.save_metadata_checkbox.grid(row=3, column=0, columnspan=1, pady=10)
+ self.save_map_button.grid(row=3, column=1, columnspan=1, pady=10)
+
+ # Preview
+ self.canvas.grid(row=0, column=2, rowspan=4, pady=10, padx=30)
+
+ # Color in which we want to draw the lines
+ self.primary_color = (255, 255, 255) # white
+
+ # Render initial map
+ self.root.update() # We need to call update() first
+ self.update_map()
+
+ def load_config(self):
+ # Prompt the user to select a file (force yaml)
+ file = filedialog.askopenfile(
+ mode='r',
+ defaultextension='.yaml',
+ filetypes=(('yaml file', '*.yaml'), ('All Files', '*.*')),
+ )
+ if file:
+ # Load the config file
+ config = load_config_file(file)
+ if config is None:
+ # Show error box and return if the file is invalid
+ tk.messagebox.showerror('Error', 'Invalid config file')
+ return
+ # Set the parameters in the gui
+ for parameter_name, parameter_value in config.items():
+ self.parameter_input.parameter_values[parameter_name].set(
+ parameter_value
+ )
+ # Update the map
+ self.update_map()
+
+ def save_config(self):
+ # Get the parameters from the user input
+ parameters = self.parameter_input.get_parameters()
+ # Open a file dialog to select the file
+ file = filedialog.asksaveasfile(
+ mode='w',
+ defaultextension='.yaml',
+ filetypes=(('yaml file', '*.yaml'), ('All Files', '*.*')),
+ )
+ if file:
+ # Add header
+ file.write('# Map Generator Config\n')
+ file.write('# This file was generated by the map generator GUI\n\n')
+ # Save the parameters in this format:
+ yaml.dump(
+ {
+ 'header': {'version': '1.0', 'type': 'map_generator_config'},
+ 'parameters': parameters,
+ },
+ file,
+ sort_keys=False,
+ )
+ print(f'Saved config to {file.name}')
+
+ def save_map(self):
+ file = filedialog.asksaveasfile(
+ mode='w',
+ defaultextension='.png',
+ filetypes=(('png file', '*.png'), ('All Files', '*.*')),
+ )
+ if file:
+ print(f'Saving map to {file.name}')
+
+ # Generate and save the map
+ parameters = self.parameter_input.get_parameters()
+ generated_map = generate_map_image(parameters)
+ if cv2.imwrite(file.name, generated_map):
+ # Save metadata
+ if self.save_metadata.get():
+ # Save the metadata in this format:
+ metadata = generate_metadata(
+ parameters, os.path.basename(file.name)
+ )
+ # Save metadata in the same folder as the map
+ metadata_file = os.path.join(
+ os.path.dirname(file.name), 'map_server.yaml'
+ )
+ with open(metadata_file, 'w') as f:
+ yaml.dump(metadata, f, sort_keys=False)
+ print(f'Saved metadata to {metadata_file}')
+
+ # Show success box and ask if we want to open it with the default image viewer
+ if tk.messagebox.askyesno(
+ 'Success', 'Map saved successfully. Do you want to open it?'
+ ):
+ import platform
+ import subprocess
+
+ if platform.system() == 'Windows':
+ subprocess.Popen(['start', file.name], shell=True)
+ elif platform.system() == 'Darwin':
+ subprocess.Popen(['open', file.name])
+ else:
+ subprocess.Popen(['xdg-open', file.name])
+ else:
+ # Show error box
+ tk.messagebox.showerror('Error', 'Could not save map to file')
+
+ def update_map(self, *args):
+ # Generate and display the map on the canvas
+ try:
+ generated_map = generate_map_image(self.parameter_input.get_parameters())
+ self.display_map(generated_map)
+ except tk.TclError as e:
+ print(f"Invalid input for map generation. '{e}'")
+
+ def display_map(self, image):
+ # Display the generated map on the canvas
+ img = Image.fromarray(image)
+ # Resize to fit canvas while keeping aspect ratio
+ img.thumbnail(
+ (self.canvas.winfo_width(), self.canvas.winfo_height()),
+ Image.Resampling.LANCZOS,
+ )
+ img = ImageTk.PhotoImage(img)
+ self.canvas.create_image(0, 0, anchor=tk.NW, image=img)
+ self.canvas.image = img # To prevent garbage collection
+
+
+def main():
+ root = tk.Tk()
+ MapGeneratorGUI(root)
+ root.mainloop()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/soccer_field_map_generator/soccer_field_map_generator/tooltip.py b/soccer_field_map_generator/soccer_field_map_generator/tooltip.py
new file mode 100644
index 0000000..c71ac0a
--- /dev/null
+++ b/soccer_field_map_generator/soccer_field_map_generator/tooltip.py
@@ -0,0 +1,151 @@
+import tkinter as tk
+
+
+class Tooltip:
+ """
+ It creates a tooltip for a given widget as the mouse goes on it.
+
+ see:
+
+ http://stackoverflow.com/questions/3221956/
+ what-is-the-simplest-way-to-make-tooltips-
+ in-tkinter/36221216#36221216
+
+ http://www.daniweb.com/programming/software-development/
+ code/484591/a-tooltip-class-for-tkinter
+
+ - Originally written by vegaseat on 2014.09.09.
+
+ - Modified to include a delay time by Victor Zaccardo on 2016.03.25.
+
+ - Modified
+ - to correct extreme right and extreme bottom behavior,
+ - to stay inside the screen whenever the tooltip might go out on
+ the top but still the screen is higher than the tooltip,
+ - to use the more flexible mouse positioning,
+ - to add customizable background color, padding, waittime and
+ wraplength on creation
+ by Alberto Vassena on 2016.11.05.
+
+ Tested on Ubuntu 16.04/16.10, running Python 3.5.2
+
+ TODO: themes styles support
+ """
+
+ def __init__(
+ self,
+ widget,
+ *,
+ bg='#FFFFEA',
+ pad=(5, 3, 5, 3),
+ text='widget info',
+ waittime=400,
+ wraplength=250
+ ):
+ self.waittime = waittime # in miliseconds, originally 500
+ self.wraplength = wraplength # in pixels, originally 180
+ self.widget = widget
+ self.text = text
+ self.widget.bind('', self.onEnter)
+ self.widget.bind('', self.onLeave)
+ self.widget.bind('', self.onLeave)
+ self.bg = bg
+ self.pad = pad
+ self.id = None
+ self.tw = None
+
+ def onEnter(self, event=None):
+ self.schedule()
+
+ def onLeave(self, event=None):
+ self.unschedule()
+ self.hide()
+
+ def schedule(self):
+ self.unschedule()
+ self.id = self.widget.after(self.waittime, self.show)
+
+ def unschedule(self):
+ id_ = self.id
+ self.id = None
+ if id_:
+ self.widget.after_cancel(id_)
+
+ def show(self):
+ def tip_pos_calculator(widget, label, *, tip_delta=(10, 5), pad=(5, 3, 5, 3)):
+ w = widget
+
+ s_width, s_height = w.winfo_screenwidth(), w.winfo_screenheight()
+
+ width, height = (
+ pad[0] + label.winfo_reqwidth() + pad[2],
+ pad[1] + label.winfo_reqheight() + pad[3],
+ )
+
+ mouse_x, mouse_y = w.winfo_pointerxy()
+
+ x1, y1 = mouse_x + tip_delta[0], mouse_y + tip_delta[1]
+ x2, y2 = x1 + width, y1 + height
+
+ x_delta = x2 - s_width
+ if x_delta < 0:
+ x_delta = 0
+ y_delta = y2 - s_height
+ if y_delta < 0:
+ y_delta = 0
+
+ offscreen = (x_delta, y_delta) != (0, 0)
+
+ if offscreen:
+ if x_delta:
+ x1 = mouse_x - tip_delta[0] - width
+
+ if y_delta:
+ y1 = mouse_y - tip_delta[1] - height
+
+ offscreen_again = y1 < 0 # out on the top
+
+ if offscreen_again:
+ # No further checks will be done.
+
+ # TIP:
+ # A further mod might automagically augment the
+ # wraplength when the tooltip is too high to be
+ # kept inside the screen.
+ y1 = 0
+
+ return x1, y1
+
+ bg = self.bg
+ pad = self.pad
+ widget = self.widget
+
+ # creates a toplevel window
+ self.tw = tk.Toplevel(widget)
+
+ # Leaves only the label and removes the app window
+ self.tw.wm_overrideredirect(True)
+
+ win = tk.Frame(self.tw, background=bg, borderwidth=0)
+ label = tk.Label(
+ win,
+ text=self.text,
+ justify=tk.LEFT,
+ background=bg,
+ relief=tk.SOLID,
+ borderwidth=0,
+ wraplength=self.wraplength,
+ )
+
+ label.grid(padx=(pad[0], pad[2]), pady=(pad[1], pad[3]), sticky=tk.NSEW)
+ win.grid()
+
+ x, y = tip_pos_calculator(widget, label)
+
+ self.tw.wm_geometry('+%d+%d' % (x, y))
+
+ def hide(self):
+ tw = self.tw
+ if tw:
+ tw.destroy()
+ self.tw = None
diff --git a/soccer_field_map_generator/test/test_copyright.py b/soccer_field_map_generator/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/soccer_field_map_generator/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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 ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/soccer_field_map_generator/test/test_flake8.py b/soccer_field_map_generator/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/soccer_field_map_generator/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# 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 ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/soccer_field_map_generator/test/test_pep257.py b/soccer_field_map_generator/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/soccer_field_map_generator/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# 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 ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'