Skip to content

Custom floor generator #13

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
Iranaphor wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

Custom floor generator #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
688d8e5
kml2tmap accepts filepaths
Iranaphor Jun 12, 2024
637b357
included file
Iranaphor Oct 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
107 changes: 83 additions & 24 deletions environment_common/convertors/kml_to_tmap.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,9 @@ class KlmRead:

@classmethod
def polyline_to_dictlist(cls, polyline_str, name, tagtype, circuit=False):
pls = polyline_str.replace('\n','').replace('\t','').split(' ')[:-1]
pls = polyline_str.replace('\n','').replace('\t','').strip().split(' ')[:-1]
coords = [g.split(',') for g in pls]

dictlist = [{'longitude':round(float(gnss[0]),6),
'latitude': round(float(gnss[1]),6),
'elevation':round(float(gnss[2]),6),
Expand Down Expand Up @@ -109,9 +110,15 @@ def group_similar_coords(coord_dict_list):

def run(args=None):

datum_path = os.path.join(args['src'], 'config', 'location', 'datum.yaml')
if not os.path.isfile(datum_path):
datum_path = os.path.join(args['src'], 'config', 'location', 'datum_autogen.yaml')
# Identify the filepath for the datum file
if 'datum_filepath' in args:
datum_path = args['datum_filepath']
else:
datum_path = os.path.join(args['src'], 'config', 'location', 'datum.yaml')
if not os.path.isfile(datum_path):
datum_path = os.path.join(args['src'], 'config', 'location', 'datum_autogen.yaml')

# Open the datum file ot extract the datum latitude and longitude
with open(datum_path) as f:
data = f.read()
datum = yaml.safe_load(data)
Expand All @@ -121,50 +128,69 @@ def run(args=None):
lon = datum['datum_longitude']


ENV = get_package_share_directory('environment_template')
kml_path = os.path.join(args['src'], 'config', 'topological', 'raw_connections.kml')
while True:
print(f'Constructing tmap from `{kml_path}`. \nTo use a different KML, place the `.kml` file in: `environment_template/config/topological/` \n\n\nEnter the name of the file below, or press [ENTER] to continue:')
inp = input('>> environment_template/config/topological/')
print('\n')
print(inp)
if inp != '':
if not inp.endswith('.kml'):
print('Ensure you have included the correct file extension of: `.kml`\n\n')
# Identify the filepath for the kmnl file
if 'kml_filepath' in args:
kml_path = args['kml_filepath']
else:
ENV = get_package_share_directory('environment_template')
kml_path = os.path.join(args['src'], 'config', 'topological', 'raw_connections.kml')
while True:
et_fp = '`environment_template/config/topological/`'
print(f'Constructing tmap from `{kml_path}`.')
print(f'To use a different KML, place the `.kml` file in: {rt_fp}')
print(f'\n\nEnter the name of the file below, or press [ENTER] to continue:')
inp = input(f'>> {et_fp}')
print('\n')
print(inp)
if inp != '':
if not inp.endswith('.kml'):
print('Ensure you have included the correct file extension of: `.kml`\n\n')
else:
kml_path = os.path.join(args['src'], 'config', 'topological', inp)
break
else:
kml_path = os.path.join(args['src'], 'config', 'topological', inp)
break
else:
break

# Extract KML content
locations = dict()
tree = ET.parse(kml_path)

# Identify coords
root = tree.getroot()
coords = KlmRead.get_coords(root)
#pprint(coords)

# Get node locations
allpoints = sum(coords.values(),[])
#[print(f"{l['longitude']} : {l['latitude']}) \t-- {l['raw_name']} {l['raw_connections']}") for l in allpoints]

# Group coords together if close
lesspoints = group_similar_coords(allpoints)
print(lesspoints)

# Calculate local coordinates
for l in lesspoints:
l['y'], l['x'] = calculate_distance_changes(lat, lon, l['latitude'], l['longitude'])
[print(f"{l['name']} ({l['longitude']}:{l['latitude']}) - {l['connections']}") for l in lesspoints]
print(f"Reduced {len(allpoints)} raw points down to {len(lesspoints)}")

#[print(f"{l['name']} ({round(l['x'],1)}:{round(l['y'],1)}) - {l['connections']}") for l in lesspoints]

# Begin formatting tmap information
tmap = TMapTemplates.vert_sample
#tmap += TMapTemplates.vert_opening
#tmap += TMapTemplates.vert_ring.format(**{'id':'vert2', 'sz':1})
print('|\n|\n|\n|\n|', place_id)
tmap += TMapTemplates.opening.format(**{'gen_time':0, 'location':place_id})

# Set default values for nodes and edges
node = {'location':place_id, 'vert': 'vert1', 'restrictions':'robot', 'connections':None}
edge = {'action':'move_base', 'action_type':'move_base_msgs/MoveBaseGoal', 'restrictions':'robot'}

# Process the nodes and edges
for l in lesspoints:

# Update and add node
node.update({'name':l['name'], 'x':l['x'], 'y':l['y']})
tmap += TMapTemplates.node.format(**node)

# Add edges to the node
if not l['connections']:
tmap += TMapTemplates.edges_empty
else:
Expand All @@ -173,11 +199,18 @@ def run(args=None):
edge.update({'name':l['name'], 'name2':c})
tmap += TMapTemplates.edges.format(**edge)

tmap_path = os.path.join(args['src'], 'config', 'topological', 'network_autogen.tmap2.yaml')
# Save tmap2 file
if 'tmap2_filepath' in args:
tmap_path = args['tmap2_filepath']
else:
tmap_path = os.path.join(args['src'], 'config', 'topological', 'network_autogen.tmap2.yaml')

# Write the tmap
print(tmap_path)
with open(tmap_path, 'w') as f:
f.write(tmap)

# Upload to google drive path if included
if os.getenv('GDRIVE_PATH', ""):
gdrive_path = os.path.join(os.getenv('GDRIVE_PATH'), 'Google Earth', 'kml', 'network_autogen.tmap2.yaml')
with open(gdrive_path, 'w') as f:
Expand All @@ -192,8 +225,34 @@ def main(args=None):
print('missing ENVVAR FIELD_NAME, not continuing')
return
print('Generating map for field: '+location_name)
args = {'src': src, 'location_name':location_name, 'line_col':'ff2f2fd3', 'line_width':'4', 'fill_col':'c02f2fd3', 'shape_size':0.000005}
args = {'src': src,
'location_name':location_name,
'line_col':'ff2f2fd3',
'line_width':'4',
'fill_col':'c02f2fd3',
'shape_size':0.000005}
run(args)

def main_no_template(datum_filepath, kml_filepath, tmap2_filepath):
args = {'datum_filepath': datum_filepath,
'kml_filepath': kml_filepath,
'tmap2_filepath': tmap2_filepath,
'location_name':'none',
'line_col':'ff2f2fd3',
'line_width':'4',
'fill_col':'c02f2fd3',
'shape_size':0.000005}
run(args)

if __name__ == '__main__':
main()
if len(sys.argv) >= 3:
d, k, t = sys.argv[1], sys.argv[2], sys.argv[3]
print(f'Datum filepath: {d}')
print(f' KML filepath: {k}')
print(f'TMap2 filepath: {t}')
main_no_template(d, k, t)
else:
main()



114 changes: 114 additions & 0 deletions environment_common/convertors/metric_to_sdf.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
import sys, os
import yaml
from ament_index_python.packages import get_package_share_directory, get_package_prefix
import xml.etree.ElementTree as ET
from pprint import pprint

from environment_common.convertors.templating.sdf import SDFTemplates

# consider using trace2cad.sh

def run(args=None):

# Identify the filepath for the map file
if 'map_yaml_filepath' in args:
map_yaml_path = args['map_yaml_filepath']
else:
map_yaml_path = os.path.join(args['src'], 'config', 'map', 'map', 'map.yaml')
if not os.path.isfile(map_yaml_filepath):
map_yaml_path = os.path.join(args['src'], 'config', 'map', 'map', 'map_autogen.yaml')


# Open the map yaml
with open(map_yaml_path) as f:
data = f.read()
map_yaml = yaml.safe_load(data)
image = dimensions['image']
resolution = dimensions['resolution']


# Identify the filepath for the map file
map_path = os.path.join(args['src'], 'config', 'map', 'map', image)
if map_path.endswith('.pgm'):
with open(map_path, 'rb') as file:
# Read the first two lines: PGM type and dimensions
header = file.readline() + file.readline()
if b'#' in header: # Handle comment line if present
header += file.readline()
# Decode header to parse dimensions
header = header.decode('ascii').split()
dimensions = [int(s) for s in header if s.isdigit()]
return dimensions[0], dimensions[1]

"""
1. open map yaml
2. get image file size (what about alternative image like blueprint)
3.


"""








# Save tmap2 file
if 'tmap2_filepath' in args:
tmap_path = args['tmap2_filepath']
else:
tmap_path = os.path.join(args['src'], 'config', 'topological', 'network_autogen.tmap2.yaml')

# Write the tmap
print(tmap_path)
with open(tmap_path, 'w') as f:
f.write(tmap)

# Upload to google drive path if included
if os.getenv('GDRIVE_PATH', ""):
gdrive_path = os.path.join(os.getenv('GDRIVE_PATH'), 'Google Earth', 'kml', 'network_autogen.tmap2.yaml')
with open(gdrive_path, 'w') as f:
f.write(tmap)


def main(args=None):
e = 'environment_template'
src = '/'.join(get_package_prefix(e).split('/')[:-2]) + f'/src/{e}'
location_name = os.getenv('FIELD_NAME')
if not location_name:
print('missing ENVVAR FIELD_NAME, not continuing')
return
print('Generating map for field: '+location_name)
args = {'src': src,
'location_name':location_name,
'line_col':'ff2f2fd3',
'line_width':'4',
'fill_col':'c02f2fd3',
'shape_size':0.000005}
run(args)

def main_no_template(datum_filepath, kml_filepath, tmap2_filepath):
args = {'datum_filepath': datum_filepath,
'kml_filepath': kml_filepath,
'tmap2_filepath': tmap2_filepath,
'location_name':'none',
'line_col':'ff2f2fd3',
'line_width':'4',
'fill_col':'c02f2fd3',
'shape_size':0.000005}
run(args)

if __name__ == '__main__':
if len(sys.argv) >= 3:
d, k, t = sys.argv[1], sys.argv[2], sys.argv[3]
print(f'Datum filepath: {d}')
print(f' KML filepath: {k}')
print(f'TMap2 filepath: {t}')
main_no_template(d, k, t)
else:
main()