Commit d14b7201 authored by Stefan Schuhbaeck's avatar Stefan Schuhbaeck
Browse files

add stand alone features from VadereManagement module:

* single step mode in GUI:
  Allows the user to step through the simulation one step at a time to
  identify bugs.
* simplify obstacles:
  Merge multiple obstacles based on the convex hull their points create.
  The merge can be undon
* add features to open street map (osm) importer:
  1) import 'open' paths as polygons with a specified width. With this
     it is possible to create walls or subway entrance
  2) add option to include osm ids into each obstacle
parent fd7953ae
Pipeline #125635 passed with stages
in 109 minutes and 1 second
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
...@@ -55,15 +55,172 @@ from string import Template ...@@ -55,15 +55,172 @@ from string import Template
import argparse import argparse
import utm import utm
import math import math
import numpy as np
import itertools as iter
from typing import List
vadere_obstacle_string = """{
"shape" : {
"type" : "POLYGON",
"points" : [ $points ]
},
"id" : $id
}"""
class PathToPolygon:
def __init__(self, list_of_tuples, dist):
self.points = [np.array([p[0], p[1]]) for p in list_of_tuples]
self.dist = dist
self.rotPositive = np.array(((0, -1), (1, 0)))
self.rotNegative = np.array(((0, 1), (-1, 0)))
self.polygon = []
self.create_poly_points()
@classmethod
def get_poly_object(cls, list_of_tuples, dist, id):
ptp = cls(list_of_tuples, dist)
return PolyObjectWidthId(id, ptp.create_poly_points())
def create_poly_points(self):
lines_o1 = []
lines_o2 = []
for a, b in self.pairwise(self.points):
lines = self.parallel_lines([a, b], self.dist)
lines_o1.append(lines[0])
lines_o2.append(lines[1])
path_o1 = self.offset_line(lines_o1)
path_o2 = self.offset_line(lines_o2)
self.polygon.extend(path_o1)
path_o2.reverse()
self.polygon.extend(path_o2)
if not self.polygon_closed():
self.polygon.append(self.polygon[0])
return self.polygon
def polygon_closed(self):
if len(self.polygon) < 2:
return False
return np.array_equal(self.polygon[0], self.polygon[-1])
def offset_line(self, lines):
points = [lines[0][0]]
for l1, l2 in self.pairwise(lines):
points.append(self.line_intersection(l1, l2))
points.append(lines[-1][-1]) # last line last point
return points
@staticmethod
def pairwise(iterable):
"""s -> (s0,s1), (s1,s2), (s2, s3), ..."""
a, b = iter.tee(iterable)
next(b, None)
return zip(a, b)
@staticmethod
def line_intersection(line1, line2):
"""
see https://stackoverflow.com/a/20677983
:param line1:
:param line2:
:return:
"""
x_diff = (line1[0][0] - line1[1][0], line2[0][0] - line2[1][0])
y_diff = (line1[0][1] - line1[1][1], line2[0][1] - line2[1][1])
def det(a, b):
return a[0] * b[1] - a[1] * b[0]
div = det(x_diff, y_diff)
if div == 0:
raise Exception('lines do not intersect')
d = (det(*line1), det(*line2))
x = det(d, x_diff) / div
y = det(d, y_diff) / div
return np.array([x, y])
def parallel_lines(self, line, dist):
"""
create parallel lines moved dist amount away in +-90 deg.
:param line: [p1, p2] with p1 = [x1, y1]
:return: (line_pos, line_neg) at dist from line
"""
p1, p2 = line[0], line[1]
v = p2 - p1
v_normalized = v / np.linalg.norm(v)
# +90 and strech by dist
o1 = dist * np.matmul(self.rotPositive, v_normalized)
line_o1 = [o1 + p1, o1 + p2]
# -90 and strech by dist
o2 = dist * np.matmul(self.rotNegative, v_normalized)
line_o2 = [o2 + p1, o2 + p2]
return [line_o1, line_o2]
class PolyObjectWidthId:
"""
Simple wrapper class around a list of points (order is important!) withn an identifier field (id)
"""
def __init__(self, id, utm_points):
self.id = id
self.cartesian_points = utm_points
self.base = None
def shift_points(self, base: list):
shift_in_x = -base[0]
shift_in_y = -base[1]
self.cartesian_points = [(point[0] + shift_in_x, point[1] + shift_in_y) for point in self.cartesian_points]
self.base = base
def str2bool(v):
# see https://stackoverflow.com/a/43357954
if isinstance(v, bool):
return v
if v.lower() in ('yes', 'true', 't', 'y', '1'):
return True
elif v.lower() in ('no', 'false', 'f', 'n', '0'):
return False
else:
raise argparse.ArgumentTypeError('Boolean value expected.')
def parse_command_line_arguments(): def parse_command_line_arguments():
parser = argparse.ArgumentParser(description="Convert and OpenStreetMap file to a Vadere topology description.") parser = argparse.ArgumentParser(description="Convert and OpenStreetMap file to a Vadere topology description.")
parser.add_argument("filename", type=str, nargs="?", parser.add_argument("osm_file", type=str, nargs="?",
help="An OSM map in XML format.", help="An OSM map in XML format.",
default="maps/map_hochschule_klein.osm", default="maps/map_hochschule_klein.osm",
) )
parser.add_argument("--use-osm-id", dest='use_osm_id', type=str2bool, const=True, nargs="?",
default=True, help="Set to use osm ids for obstacles")
parser.add_argument("-o", "--output", type=str, nargs="?", parser.add_argument("-o", "--output", type=str, nargs="?",
help="Specify filename if you want the output in a file.") help="Specify filename if you want the output in a file.")
# parser.add_argument("-o", "--output", type=str, nargs="?",
# help="Specify filename if you want the output in a file.")
subparser = parser.add_subparsers(help="sub-command help")
parser_converter = subparser.add_parser('convert', help='convert given osm file to Vadere topography')
parser_converter.set_defaults(main_func=main_convert)
parser_way_to_polygon = subparser.add_parser('wayToPoly', help='convert given way id to a Vadere polygon')
parser_way_to_polygon.set_defaults(main_func=main_way_to_polygon)
parser_way_to_polygon.add_argument("-d", "--path-width", dest="d", type=float, help="Specify filename if you want the output in a file.")
parser_way_to_polygon.add_argument("-w", "--way", type=int, nargs="+")
args = parser.parse_args() args = parser.parse_args()
...@@ -71,7 +228,7 @@ def parse_command_line_arguments(): ...@@ -71,7 +228,7 @@ def parse_command_line_arguments():
def extract_latitude_and_longitude_for_each_xml_node(xml_tree): def extract_latitude_and_longitude_for_each_xml_node(xml_tree):
# Select all nodes (not only buildings). # Select all convertnodes (not only buildings).
nodes = xml_tree.xpath("/osm/node") nodes = xml_tree.xpath("/osm/node")
nodes_dictionary_with_lat_and_lon = {node.get("id"): (node.get("lat"), node.get("lon")) for node in nodes} nodes_dictionary_with_lat_and_lon = {node.get("id"): (node.get("lat"), node.get("lon")) for node in nodes}
...@@ -86,6 +243,24 @@ def filter_for_buildings(xml_tree): ...@@ -86,6 +243,24 @@ def filter_for_buildings(xml_tree):
return buildings return buildings
def filter_for_barrier(xml_tree, type='wall'):
barrier = xml_tree.xpath("/osm/way[./tag/@k='barrier']")
return barrier
def get_way(xml_tree, id):
way = xml_tree.xpath(f"/osm/way[@id='{id}']")
return way
def convert_way_to_cartesian(way, lookup_table_latitude_and_longitude, assume_closed_path=True):
node_references = way.xpath("./nd")
return convert_nodes_to_cartesian_points(node_references, lookup_table_latitude_and_longitude, assume_closed_path)
def filter_for_buildings_in_relations(xml_tree): def filter_for_buildings_in_relations(xml_tree):
# Note: A relation describes a shape with "cutting holes". # Note: A relation describes a shape with "cutting holes".
...@@ -115,11 +290,16 @@ def assert_that_start_and_end_point_are_equal(node_references): ...@@ -115,11 +290,16 @@ def assert_that_start_and_end_point_are_equal(node_references):
assert node_references[0].get("ref") == node_references[-1].get("ref") assert node_references[0].get("ref") == node_references[-1].get("ref")
def convert_nodes_to_cartesian_points(nodes, lookup_table_latitude_and_longitude): def convert_nodes_to_cartesian_points(nodes, lookup_table_latitude_and_longitude, assume_closed_path):
cartesian_points = [] cartesian_points = []
# Omit last node because it should be the same as the first one. # Omit last node becausenodes it should be the same as the first one.
for node in nodes[:len(nodes) - 1]: if assume_closed_path:
max_node = len(nodes) - 1
else:
max_node = len(nodes)
for node in nodes[:max_node]:
reference = node.get("ref") reference = node.get("ref")
latitude, longitude = lookup_table_latitude_and_longitude[reference] latitude, longitude = lookup_table_latitude_and_longitude[reference]
...@@ -132,23 +312,16 @@ def convert_nodes_to_cartesian_points(nodes, lookup_table_latitude_and_longitude ...@@ -132,23 +312,16 @@ def convert_nodes_to_cartesian_points(nodes, lookup_table_latitude_and_longitude
return cartesian_points return cartesian_points
def create_vadere_obstacles_from_points(cartesian_points): def create_vadere_obstacles_from_building(building: PolyObjectWidthId):
vadere_obstacle_string = """{ vadere_point_string = ' { "x" : $x, "y" : $y }'
"shape" : {
"type" : "POLYGON",
"points" : [ $points ]
},
"id" : -1
}"""
vadere_point_string = '{ "x" : $x, "y" : $y }'
obstacle_string_template = Template(vadere_obstacle_string) obstacle_string_template = Template(vadere_obstacle_string)
point_string_template = Template(vadere_point_string) point_string_template = Template(vadere_point_string)
points_as_string = [point_string_template.substitute(x=x, y=y) for x, y in cartesian_points] points_as_string = [point_string_template.substitute(x=x, y=y) for x, y in building.cartesian_points]
points_as_string_concatenated = ",\n".join(points_as_string) points_as_string_concatenated = ",\n".join(points_as_string)
vadere_obstacle_as_string = obstacle_string_template.substitute(points=points_as_string_concatenated) vadere_obstacle_as_string = obstacle_string_template.substitute(points=points_as_string_concatenated, id=building.id)
return vadere_obstacle_as_string return vadere_obstacle_as_string
...@@ -178,62 +351,63 @@ def print_output(outputfile, output): ...@@ -178,62 +351,63 @@ def print_output(outputfile, output):
print(output, file=text_file) print(output, file=text_file)
def find_width_and_height(buildings_cartesian): def find_width_and_height(buildings: List[PolyObjectWidthId]):
# search for the highest x- and y-coordinates within the points # search for the highest x- and y-coordinates within the points
width = 0 width = 0
height = 0 height = 0
for cartesian_points in buildings_cartesian: for cartesian_points in buildings:
for point in cartesian_points: for point in cartesian_points.cartesian_points:
width = max(width, point[0]) width = max(width, point[0])
height = max(height, point[1]) height = max(height, point[1])
return math.ceil(width), math.ceil(height) return math.ceil(width), math.ceil(height)
def find_new_basepoint(buildings_cartesian):
def find_new_basepoint(buildings: List[PolyObjectWidthId]):
# "buildings_cartesian" is a list of lists!!! # "buildings_cartesian" is a list of lists!!!
# The inner list contains the (x,y) tuples! # The inner list contains the (x,y) tuples!
# search for the lowest x- and y-coordinates within the points # search for the lowest x- and y-coordinates within the points
all_points = [point for building in buildings_cartesian for point in building] all_points = [point for building in buildings for point in building.cartesian_points]
tuple_with_min_x = min(all_points, key=lambda point: point[0]) tuple_with_min_x = min(all_points, key=lambda point: point[0])
tuple_with_min_y = min(all_points, key=lambda point: point[1]) tuple_with_min_y = min(all_points, key=lambda point: point[1])
return (tuple_with_min_x[0], tuple_with_min_y[1]) return tuple_with_min_x[0], tuple_with_min_y[1]
def shift_points(buildings_utm, shift_in_x_direction, shift_in_y_direction): def shift_way(way, base):
new_buildings = [] shift_in_x_direction = -base[0]
for cartesian_points in buildings_utm: shift_in_y_direction = -base[1]
shifted_cartesian_points = \ shift_cartesian_points = [(point[0] + shift_in_x_direction, point[1] + shift_in_y_direction) for point in way]
[(point[0] + shift_in_x_direction, point[1] + shift_in_y_direction) for point in cartesian_points] return shift_cartesian_points
new_buildings.append(shifted_cartesian_points)
return new_buildings
def convert_buildings_to_cartesian(buildings_as_xml_nodes): def convert_buildings_to_cartesian(buildings_as_xml_nodes, nodes_dictionary_with_lat_and_lon, use_osm_id: bool):
buildings_in_cartesian = [] buildings_in_cartesian = []
for building in buildings_as_xml_nodes: for building in buildings_as_xml_nodes:
# Collect nodes that belong to the current building. # Collect nodes that belong to the current building.
node_references = building.xpath("./nd") node_references = building.xpath("./nd")
assert_that_start_and_end_point_are_equal(node_references) assert_that_start_and_end_point_are_equal(node_references)
cartesian_points = convert_nodes_to_cartesian_points(node_references, nodes_dictionary_with_lat_and_lon) cartesian_points = convert_nodes_to_cartesian_points(node_references, nodes_dictionary_with_lat_and_lon,
assume_closed_path=True)
buildings_in_cartesian.append(cartesian_points) if use_osm_id:
buildings_in_cartesian.append(PolyObjectWidthId(building.get('id'), cartesian_points))
else:
buildings_in_cartesian.append(PolyObjectWidthId(-1, cartesian_points))
return buildings_in_cartesian return buildings_in_cartesian
def convert_buildings_as_cartesian_to_buildings_as_vadere_obstacles(buildings_as_cartesian): def convert_buildings_as_cartesian_to_buildings_as_vadere_obstacles(buildings: List[PolyObjectWidthId]):
list_of_vadere_obstacles_as_strings = [] list_of_vadere_obstacles_as_strings = []
for cartesian_points in buildings_as_cartesian: for building in buildings:
vadere_obstacles_as_strings = create_vadere_obstacles_from_points(cartesian_points) vadere_obstacles_as_strings = create_vadere_obstacles_from_building(building)
list_of_vadere_obstacles_as_strings.append(vadere_obstacles_as_strings) list_of_vadere_obstacles_as_strings.append(vadere_obstacles_as_strings)
return list_of_vadere_obstacles_as_strings return list_of_vadere_obstacles_as_strings
if __name__ == "__main__": def init(args):
args = parse_command_line_arguments() xml_tree = etree.parse(args.osm_file)
xml_tree = etree.parse(args.filename)
nodes_dictionary_with_lat_and_lon = extract_latitude_and_longitude_for_each_xml_node(xml_tree) nodes_dictionary_with_lat_and_lon = extract_latitude_and_longitude_for_each_xml_node(xml_tree)
...@@ -241,13 +415,28 @@ if __name__ == "__main__": ...@@ -241,13 +415,28 @@ if __name__ == "__main__":
complex_buildings = filter_for_buildings_in_relations(xml_tree) complex_buildings = filter_for_buildings_in_relations(xml_tree)
extracted_base_point = extract_base_point(xml_tree) extracted_base_point = extract_base_point(xml_tree)
print_xml_parsing_statistics(args.filename, nodes_dictionary_with_lat_and_lon, simple_buildings, complex_buildings, extracted_base_point) print_xml_parsing_statistics(args.osm_file, nodes_dictionary_with_lat_and_lon, simple_buildings, complex_buildings, extracted_base_point)
buildings_as_cartesian = convert_buildings_to_cartesian(simple_buildings + complex_buildings) buildings_as_cartesian = convert_buildings_to_cartesian(simple_buildings + complex_buildings, nodes_dictionary_with_lat_and_lon, args.use_osm_id)
# make sure everything lies within the topography
new_base = find_new_basepoint(buildings_as_cartesian) new_base = find_new_basepoint(buildings_as_cartesian)
buildings_as_cartesian = shift_points(buildings_as_cartesian, -new_base[0], -new_base[1])
for b in buildings_as_cartesian:
b.shift_points(new_base)
return xml_tree, nodes_dictionary_with_lat_and_lon, buildings_as_cartesian, new_base
def main_convert(args):
"""
osm2vadere.pu mf.osm convert -h // for sub command specific help
osm2vadere.py mf.osm convert --output map.json
"""
print(args)
xml_tree, _, buildings_as_cartesian, _ = init(args)
# make sure everything lies within the topography
width_topography, height_topography = find_width_and_height(buildings_as_cartesian) width_topography, height_topography = find_width_and_height(buildings_as_cartesian)
list_of_vadere_obstacles_as_strings = convert_buildings_as_cartesian_to_buildings_as_vadere_obstacles(buildings_as_cartesian) list_of_vadere_obstacles_as_strings = convert_buildings_as_cartesian_to_buildings_as_vadere_obstacles(buildings_as_cartesian)
...@@ -256,3 +445,32 @@ if __name__ == "__main__": ...@@ -256,3 +445,32 @@ if __name__ == "__main__":
vadere_topography_output = build_vadere_topography_input_with_obstacles(obstacles_joined, width_topography, height_topography) vadere_topography_output = build_vadere_topography_input_with_obstacles(obstacles_joined, width_topography, height_topography)
print_output(args.output, vadere_topography_output) print_output(args.output, vadere_topography_output)
def main_way_to_polygon(args):
"""
osm2vadere.py mf.osm wayToPoly -h // for sub command specific help
osm2vadere.py mf.osm wayToPoly --way 116531500 --path-width 0.25
"""
print('way_to_polygon:', args)
xml_tree, node_dict, buildings_as_cartesian, new_base = init(args)
obstacles = []
for w in args.way:
way = get_way(xml_tree, w)
assert len(way) == 1
path_list = convert_way_to_cartesian(way[0], node_dict, assume_closed_path=False)
path_list = shift_way(path_list, new_base)
vadere_obstacle = create_vadere_obstacles_from_building(PathToPolygon.get_poly_object(path_list, args.d, w))
obstacles.append(vadere_obstacle)
print_output(args.output, '\n'.join(obstacles))
return obstacles
if __name__ == "__main__":
args = parse_command_line_arguments()
# map_mf_small.osm wayToPoly -w 258139211 -d 0.5
args.main_func(args)
...@@ -3,6 +3,35 @@ from lxml import etree ...@@ -3,6 +3,35 @@ from lxml import etree
import osm2vadere import osm2vadere
import unittest import unittest
import utm import utm
import os
TEST_DATA_LON_LAT = os.path.join(os.path.dirname(__file__), 'maps/map_for_testing.osm')
TEST_DATA_2 = os.path.join(os.path.dirname(__file__), 'maps/map_mf_small.osm')
TEST_DATA="""{
"shape" : {
"type" : "POLYGON",
"points" : [ { "x" : 143.92115343943564, "y" : 168.69565355275918 },
{ "x" : 143.8295708812843, "y" : 158.46319241869656 },
{ "x" : 147.9524496438901, "y" : 158.453088570111 },
{ "x" : 148.04042161765545, "y" : 168.57734735060853 },
{ "x" : 148.54040274306163, "y" : 168.57300290155786 },
{ "x" : 148.44811333818495, "y" : 157.95187235651085 },
{ "x" : 143.3250868078719, "y" : 157.96442724530158 },
{ "x" : 143.42117346474575, "y" : 168.70012847277175 },
{ "x" : 143.92115343943564, "y" : 168.69565355275918 },
{ "x" : 143.92115343943564, "y" : 168.69565355275918 },
{ "x" : 143.8295708812843, "y" : 158.46319241869656 },
{ "x" : 147.9524496438901, "y" : 158.453088570111 },
{ "x" : 148.04042161765545, "y" : 168.57734735060853 },
{ "x" : 148.54040274306163, "y" : 168.57300290155786 },
{ "x" : 148.44811333818495, "y" : 157.95187235651085 },
{ "x" : 143.3250868078719, "y" : 157.96442724530158 },
{ "x" : 143.42117346474575, "y" : 168.70012847277175 },
{ "x" : 143.92115343943564, "y" : 168.69565355275918 } ]
},
"id" : 258139209
}
"""
class TestOsm2vadere(unittest.TestCase): class TestOsm2vadere(unittest.TestCase):
...@@ -24,7 +53,7 @@ class TestOsm2vadere(unittest.TestCase): ...@@ -24,7 +53,7 @@ class TestOsm2vadere(unittest.TestCase):
self.assertTrue(y_distance > 258 and y_distance < 275) self.assertTrue(y_distance > 258 and y_distance < 275)
def test_extract_latitude_and_longitude_for_each_xml_node(self): def test_extract_latitude_and_longitude_for_each_xml_node(self):
xml_tree = etree.parse("maps/map_for_testing.osm") xml_tree = etree.parse(TEST_DATA_LON_LAT)
nodes_dictionary_with_lat_and_lon = osm2vadere.extract_latitude_and_longitude_for_each_xml_node(xml_tree) nodes_dictionary_with_lat_and_lon = osm2vadere.extract_latitude_and_longitude_for_each_xml_node(xml_tree)
self.assertTrue(nodes_dictionary_with_lat_and_lon.get("1")[0] == "1.1") self.assertTrue(nodes_dictionary_with_lat_and_lon.get("1")[0] == "1.1")
...@@ -34,12 +63,14 @@ class TestOsm2vadere(unittest.TestCase): ...@@ -34,12 +63,14 @@ class TestOsm2vadere(unittest.TestCase):
self.assertTrue (nodes_dictionary_with_lat_and_lon.get("3")[0] == "3.1") self.assertTrue (nodes_dictionary_with_lat_and_lon.get("3")[0] == "3.1")
self.assertTrue (nodes_dictionary_with_lat_and_lon.get("3")[1] == "3.2") self.assertTrue (nodes_dictionary_with_lat_and_lon.get("3")[1] == "3.2")
def test_find_width_and_height(self): def test_find_width_and_height(self):
building_normal = [(1, 1), (3, 1), (1, 3), (3, 3)] building_normal = [(1, 1), (3, 1), (1, 3), (3, 3)]
building_negative_coordinates = [(-1, 4), (-3, 2), (10, 2)] building_negative_coordinates = [(-1, 4), (-3, 2), (10, 2)]
building_with_floating_points = [(2.3, 1.4), (-10.5, 7), (9.99, 3), (5, 7.1), (3, 4)] building_with_floating_points = [(2.3, 1.4), (-10.5, 7), (9.99, 3), (5, 7.1), (3, 4)]
buildings_cartesian = [building_normal, building_negative_coordinates, building_with_floating_points] building_points = [building_normal, building_negative_coordinates, building_with_floating_points]
width, height = osm2vadere.find_width_and_height(buildings_cartesian) buildings = [osm2vadere.PolyObjectWidthId(-1, building) for building in building_points]
width, height = osm2vadere.find_width_and_height(buildings)
self.assertTrue(width == 10) self.assertTrue(width == 10)
self.assertTrue(height == 8) # 7.1 is the maximum but the function returns math.ceil self.assertTrue(height == 8) # 7.1 is the maximum but the function returns math.ceil
...@@ -48,26 +79,45 @@ class TestOsm2vadere(unittest.TestCase): ...@@ -48,26 +79,45 @@ class TestOsm2vadere(unittest.TestCase):
building_normal = [(1, 1), (3, 1), (1, 3), (3, 3)] building_normal = [(1, 1), (3, 1), (1, 3), (3, 3)]
building_negative_coordinates = [(-1, 4), (-3, 2), (10, 2)] building_negative_coordinates = [(-1, 4), (-3, 2), (10, 2)]
building_with_floating_points = [(2.3, 1.4), (-10.5, 7), (9.99, 3), (5, 7.1), (3, 4)] building_with_floating_points = [(2.3, 1.4), (-10.5, 7), (9.99, 3), (5, 7.1), (3, 4)]
buildings_cartesian = [building_normal, building_negative_coordinates, building_with_floating_points] building_points = [building_normal, building_negative_coordinates, building_with_floating_points]
new_base_point = osm2vadere.find_new_basepoint(buildings_cartesian) buildings = [osm2vadere.PolyObjectWidthId(-1, building) for building in building_points]
new_base_point = osm2vadere.find_new_basepoint(buildings)