def _on_record_entered_clicked(self):
self.lat = self._widget.findChild(QLineEdit, 'lat_in').displayText()
self.long = self._widget.findChild(QLineEdit, 'long_in').displayText()
self.alt = self._widget.findChild(QLineEdit, 'alt_in').displayText()
self.name = self._widget.findChild(QLineEdit, 'waypoint_name').displayText()
self._widget.findChild(QLineEdit, 'lat_in').clear()
self._widget.findChild(QLineEdit, 'long_in').clear()
self._widget.findChild(QLineEdit, 'alt_in').clear()
self._widget.findChild(QLineEdit, 'waypoint_name').clear()
ecef = ecef_from_latlongheight(float(self.lat), float(self.long), float(self.alt))
global tasks
if str(self.name) in ['rings','buoys','button','spock','dock']:
for i in tasks:
if (i[0] == str(self.name)):
tasks.remove(i)
tasks.append([str(self.name),[float(ecef[0]),float(ecef[1]),float(ecef[2])]])
with open(path,'w') as task_loc:
for i in tasks:
task_loc.write(str(i[0])+','+str(i[1][0])+','+str(i[1][1])+','+str(i[1][2])+'\n')
self._widget.findChild(QListWidget, 'waypoint_list').clear()
for i in tasks:
self._widget.findChild(QListWidget, 'waypoint_list').addItem(str(i[0])+','+str(i[1][0])+','+str(i[1][1])+','+str(i[1][2]))
def lla(self, point):
'''
Produces enu, ecef, and lla from an lla point.
'''
lla = point
ecef = ecef_from_latlongheight(*np.radians(lla))
ecef_vector = ecef - self.ecef_pos
enu_vector = enu_from_ecef_tf(self.ecef_pos).dot(ecef_vector)
enu = enu_vector + self.enu_pos
return enu, ecef, lla
def read_task_pos(task):
path = roslib.packages.resource_file('gps_waypoints','saved','task_loc_'+course+'.txt')
with open(path,'r+') as task_loc:
for line in task_loc:
data = line.split(',')
if data[0] == task:
return [float(data[1]),float(data[2]),float(data[3])]
return ecef_from_latlongheight(1.0,2.0,3.0)
def lla(self, point):
'''
Produces enu, ecef, and lla from an lla point.
'''
lla = point
ecef = ecef_from_latlongheight(*np.radians(lla))
ecef_vector = ecef - self.ecef_pos
enu_vector = enu_from_ecef_tf(self.ecef_pos).dot(ecef_vector)
enu = enu_vector + self.enu_pos
return enu, ecef, lla
def read_task_pos(task):
path = roslib.packages.resource_file('gps_waypoints', 'saved',
'task_loc_' + course + '.txt')
with open(path, 'r+') as task_loc:
for line in task_loc:
data = line.split(',')
if data[0] == task:
return [float(data[1]), float(data[2]), float(data[3])]
return ecef_from_latlongheight(1.0, 2.0, 3.0)
def lla(self):
lla = self.point
# to ecef
ecef = ecef_from_latlongheight(*np.radians(lla))
# to enu
ecef_vector = ecef - self.ecef_pos
enu_vector = enu_from_ecef_tf(self.ecef_pos).dot(ecef_vector)
enu = enu_vector + self.enu_pos
return enu, ecef, lla
def lla(self):
lla = self.point
# to ecef
ecef = ecef_from_latlongheight(*np.radians(lla))
# to enu
ecef_vector = ecef - self.ecef_pos
enu_vector = enu_from_ecef_tf(self.ecef_pos).dot(ecef_vector)
enu = enu_vector + self.enu_pos
return enu, ecef, lla
def to_lat_long(self, lat, lon, alt=0):
"""
Goes to a lat long position and keep the same orientation
Note: lat and long need to be degrees
"""
ecef_pos, enu_pos = self.nav.ecef_pose[0], self.nav.pose[0]
# These functions want radians
lat, lon = np.radians([lat, lon], dtype=np.float64)
ecef_vector = ecef_from_latlongheight(lat, lon, alt) - ecef_pos
enu_vector = enu_from_ecef(ecef_vector, ecef_pos)
enu_vector[2] = 0 # We don't want to move in the z at all
return self.set_position(enu_pos + enu_vector)
def to_lat_long(self, lat, lon, alt=0):
"""
Goes to a lat long position and keep the same orientation
Note: lat and long need to be degrees
"""
ecef_pos, enu_pos = self.nav.ecef_pose[0], self.nav.pose[0]
# These functions want radians
lat, lon = np.radians([lat, lon], dtype=np.float64)
ecef_vector = ecef_from_latlongheight(lat, lon, alt) - ecef_pos
enu_vector = enu_from_ecef(ecef_vector, ecef_pos)
enu_vector[2] = 0 # We don't want to move in the z at all
return self.set_position(enu_pos + enu_vector)
def __init__(self, target='wamv::base_link'):
self.target = target
intial_lla = [29.534912, -82.303642, 0] # In Wauhburg
self.last_ecef = gps.ecef_from_latlongheight(*np.radians(intial_lla))
self.last_enu = None
self.last_odom = None
self.position_offset = None
self.state_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.state_cb)
self.state_pub = rospy.Publisher('model_odom', Odometry, queue_size=1)
self.absstate_pub = rospy.Publisher('absodom', Odometry, queue_size=1)
self.last_odom = None
self.last_absodom = None
rospy.Timer(rospy.Duration(1/100), self.publish_odom)
def __init__(self, target='wamv::base_link'):
self.target = target
intial_lla = [29.534912, -82.303642, 0] # In Wauhburg
self.last_ecef = gps.ecef_from_latlongheight(*np.radians(intial_lla))
self.last_enu = None
self.last_odom = None
self.position_offset = None
self.state_sub_max_prd = rospy.Duration(1 / 100) # s
self.last_state_sub_time = rospy.Time.now()
self.state_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.state_cb)
self.state_pub = rospy.Publisher('model_odom', Odometry, queue_size=1)
self.absstate_pub = rospy.Publisher('absodom', Odometry, queue_size=1)
self.last_odom = None
self.last_absodom = None
rospy.Timer(rospy.Duration(1 / 100), self.publish_odom)
def __init__(self, target='wamv::base_link'):
self.target = target
intial_lla = [29.534912, -82.303642, 0] # In Walhburg
self.last_ecef = gps.ecef_from_latlongheight(*np.radians(intial_lla))
self.last_enu = None
self.last_odom = None
self.position_offset = None
self.state_sub = rospy.Subscriber('/gazebo/link_states', LinkStates,
self.state_cb)
self.state_pub = rospy.Publisher(
'odom', Odometry, queue_size=1) # This can be thought of as ENU
self.absstate_pub = rospy.Publisher(
'absodom', Odometry,
queue_size=1) # TODO: Make this in ECEF frame instead of ENU
self.last_odom = None
self.last_absodom = None
rospy.Timer(rospy.Duration(1 / 100), self.publish_odom)
def _on_record_entered_clicked(self):
self.lat = self._widget.findChild(QLineEdit, 'lat_in').displayText()
self.long = self._widget.findChild(QLineEdit, 'long_in').displayText()
self.alt = self._widget.findChild(QLineEdit, 'alt_in').displayText()
self.name = self._widget.findChild(QLineEdit,
'waypoint_name').displayText()
self._widget.findChild(QLineEdit, 'lat_in').clear()
self._widget.findChild(QLineEdit, 'long_in').clear()
self._widget.findChild(QLineEdit, 'alt_in').clear()
self._widget.findChild(QLineEdit, 'waypoint_name').clear()
ecef = ecef_from_latlongheight(float(self.lat), float(self.long),
float(self.alt))
global tasks
if str(self.name) in ['rings', 'buoys', 'button', 'spock', 'dock']:
for i in tasks:
if (i[0] == str(self.name)):
tasks.remove(i)
tasks.append([
str(self.name),
[float(ecef[0]),
float(ecef[1]),
float(ecef[2])]
])
with open(path, 'w') as task_loc:
for i in tasks:
task_loc.write(
str(i[0]) + ',' + str(i[1][0]) + ',' + str(i[1][1]) + ',' +
str(i[1][2]) + '\n')
self._widget.findChild(QListWidget, 'waypoint_list').clear()
for i in tasks:
self._widget.findChild(QListWidget, 'waypoint_list').addItem(
str(i[0]) + ',' + str(i[1][0]) + ',' + str(i[1][1]) + ',' +
str(i[1][2]))
def ll(lat, lon):
return gps.ecef_from_latlongheight(math.radians(lat), math.radians(lon), 0)
def ll(lat, lon):
return gps.ecef_from_latlongheight(math.radians(lat), math.radians(lon), 0)
