mirror of
https://github.com/CopterExpress/clover.git
synced 2026-05-27 13:39:33 +00:00
44 lines
1.2 KiB
Python
44 lines
1.2 KiB
Python
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
|