From 621b1ae5b6166cd88f8ca7a6fb46c9e85e8a3542 Mon Sep 17 00:00:00 2001 From: Diptodip Deb Date: Tue, 28 Feb 2017 19:39:31 -0500 Subject: [PATCH 1/5] Gazebo to lat-lons and I can't use git --- .../models/gzb_utils/gzb_to_latlon.py | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 buzzmobile/simulation/models/gzb_utils/gzb_to_latlon.py 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() From f929ae4c30aed7b125498e8b4edf5b8b7e3a40a5 Mon Sep 17 00:00:00 2001 From: Diptodip Deb Date: Tue, 7 Mar 2017 19:42:12 -0500 Subject: [PATCH 2/5] Convert list of road coordinates to list of latitudes and longitudes --- .../simulation/models/gzb_utils/world_to_polyline.py | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 buzzmobile/simulation/models/gzb_utils/world_to_polyline.py 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..ed04219 --- /dev/null +++ b/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py @@ -0,0 +1,9 @@ +import xml.etree.ElementTree as ET +from gzb_to_latlon import convert + +def straight_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 From 9cf6c79176403e890030600859f0a59e74832f84 Mon Sep 17 00:00:00 2001 From: Diptodip Deb Date: Tue, 7 Mar 2017 19:55:49 -0500 Subject: [PATCH 3/5] Begin parsing world XML files --- buzzmobile/simulation/models/gzb_utils/world_to_polyline.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py b/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py index ed04219..8f03f68 100644 --- a/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py +++ b/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py @@ -7,3 +7,9 @@ def straight_road_coordinates_to_latlons(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: + print (child.tag, child.attrib) From c36e491eb164c537b215601af933a1399e9157d5 Mon Sep 17 00:00:00 2001 From: Diptodip Deb Date: Tue, 28 Mar 2017 23:47:08 -0400 Subject: [PATCH 4/5] Add camera element to XML world file --- .../models/gzb_utils/world_to_polyline.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py b/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py index 8f03f68..1dbe342 100644 --- a/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py +++ b/buzzmobile/simulation/models/gzb_utils/world_to_polyline.py @@ -1,7 +1,9 @@ +import sys +import xml import xml.etree.ElementTree as ET from gzb_to_latlon import convert -def straight_road_coordinates_to_latlons(road_coordinates): +def road_coordinates_to_latlons(road_coordinates): latlons = [] for coordinate in road_coordinates: (lat, lon) = convert(coordinate[0], coordinate[1]) @@ -12,4 +14,14 @@ def world_xml_to_road_coordinates(filename): tree = ET.parse(filename) root = tree.getroot() for child in root: - print (child.tag, child.attrib) + 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]) From e0511b5653307cd31ac6dae983aea933f079684d Mon Sep 17 00:00:00 2001 From: Diptodip Deb Date: Tue, 4 Apr 2017 19:12:48 -0400 Subject: [PATCH 5/5] Save forward Polyline --- buzzmobile/process/gps_mapper/gps_mapper.lisp | 3 +++ buzzmobile/simulation/models/gzb_utils/straight.polyline | 1 + 2 files changed, 4 insertions(+) create mode 100644 buzzmobile/process/gps_mapper/gps_mapper.lisp create mode 100644 buzzmobile/simulation/models/gzb_utils/straight.polyline 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/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?