diff --git a/buzzmobile/process/gps_mapper/gps_mapper.lisp b/buzzmobile/process/gps_mapper/gps_mapper.lisp new file mode 100644 index 0000000..4a1576e --- /dev/null +++ b/buzzmobile/process/gps_mapper/gps_mapper.lisp @@ -0,0 +1,3 @@ +(with-ros-node ("gps_mapper_node") + (do-stuff) + ) diff --git a/buzzmobile/simulation/models/gzb_utils/gzb_to_latlon.py b/buzzmobile/simulation/models/gzb_utils/gzb_to_latlon.py new file mode 100644 index 0000000..ac90823 --- /dev/null +++ b/buzzmobile/simulation/models/gzb_utils/gzb_to_latlon.py @@ -0,0 +1,30 @@ +from math import pi, sin, cos, sqrt +import sys + +EARTH_RADIUS = 6378137.0 # we're not using params because this is gzb specific +FLATTENING = 1.0/298.257223563 +EXCENTRICITY2 = 2 * FLATTENING - (FLATTENING**2) + +REF_LAT = 49.9 +REF_LON = 8.9 +REF_HEAD = 0.0 +REF_ALT = 0.0 + +TEMP = 1.0 / (1.0 - EXCENTRICITY2 * sin(REF_LAT * pi/180.0) * sin(REF_LAT * pi/180.0)) +PRIME_VERT_RADIUS = EARTH_RADIUS * sqrt(TEMP) +NORTH_RADIUS = PRIME_VERT_RADIUS * (1 - EXCENTRICITY2) * TEMP +EAST_RADIUS = PRIME_VERT_RADIUS * cos(REF_LAT * pi/180.0) + +def convert(x, y): + lat = REF_LAT + ((cos(REF_HEAD) * x + sin(REF_HEAD) * y) / NORTH_RADIUS) * 180.0/pi + lon = REF_LON - ((-sin(REF_HEAD) * x + cos(REF_HEAD) * y) / EAST_RADIUS) * 180.0/pi + return (lat, lon) + +def main(): + print("[note] expecting arguments as: x y") + x = float(sys.argv[1]) + y = float(sys.argv[2]) + (lat, lon) = convert(x, y) + print("[out] {0} {1}".format(lat, lon)) + +if __name__=='__main__': main() diff --git a/buzzmobile/simulation/models/gzb_utils/straight.polyline b/buzzmobile/simulation/models/gzb_utils/straight.polyline new file mode 100644 index 0000000..94dea15 --- /dev/null +++ b/buzzmobile/simulation/models/gzb_utils/straight.polyline @@ -0,0 +1 @@ +_bqoH_hiu@cQ? diff --git a/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py b/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py new file mode 100644 index 0000000..1dbe342 --- /dev/null +++ b/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py @@ -0,0 +1,27 @@ +import sys +import xml +import xml.etree.ElementTree as ET +from gzb_to_latlon import convert + +def road_coordinates_to_latlons(road_coordinates): + latlons = [] + for coordinate in road_coordinates: + (lat, lon) = convert(coordinate[0], coordinate[1]) + latlons.append((lat, lon)) + return latlons + +def world_xml_to_road_coordinates(filename): + tree = ET.parse(filename) + root = tree.getroot() + for child in root: + if 'world' == str(child.tag): + gui = xml.Element('gui') + camera = xml.Element('camera') + pose = xml.Element('pose') + pose.text = '0 0 50 0 0 0' + camera.append(pose) + gui.append(camera) + child.append(gui) + + +if __name__ == '__main__': world_xml_to_road_coordinates(sys.argv[1])