From b88dd6cdf268e8f97dd80e8af96b699e1c04a1f7 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 20 Jan 2019 20:48:31 +0300 Subject: [PATCH] Remove unused global_local.py --- clever/src/global_local.py | 43 -------------------------------------- 1 file changed, 43 deletions(-) delete mode 100644 clever/src/global_local.py diff --git a/clever/src/global_local.py b/clever/src/global_local.py deleted file mode 100644 index dd5f7d85..00000000 --- a/clever/src/global_local.py +++ /dev/null @@ -1,43 +0,0 @@ -import rospy -import math -import geopy -from geometry_msgs.msg import PoseStamped -from geopy.distance import VincentyDistance, vincenty -from sensor_msgs.msg import NavSatFix - - -def global_to_local(lat, lon): - # TODO: refactor - - try: - position_global = rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.5) - except rospy.exceptions.ROSException: - raise Exception('No global position') - - try: - pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=0.5) - except rospy.exceptions.ROSException: - raise Exception('No local position') - - d = math.hypot(pose.pose.position.x, pose.pose.position.y) - - bearing = math.degrees(math.atan2(-pose.pose.position.x, -pose.pose.position.y)) - if bearing < 0: - bearing += 360 - - cur = geopy.Point(position_global.latitude, position_global.longitude) - origin = VincentyDistance(meters=d).destination(cur, bearing) - - _origin = origin.latitude, origin.longitude - olat_tlon = origin.latitude, lon - tlat_olon = lat, origin.longitude - - N = vincenty(_origin, tlat_olon) - if lat < origin.latitude: - N = -N - - E = vincenty(_origin, olat_tlon) - if lon < origin.longitude: - E = -E - - return E.meters, N.meters