Exemplo n.º 1
0
    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
Exemplo n.º 3
0
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)
Exemplo n.º 4
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
Exemplo n.º 5
0
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
Exemplo n.º 8
0
    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)
Exemplo n.º 9
0
    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)
Exemplo n.º 10
0
    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)
Exemplo n.º 11
0
    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)
Exemplo n.º 12
0
    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)
Exemplo n.º 13
0
    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]))
Exemplo n.º 14
0
def ll(lat, lon):
    return gps.ecef_from_latlongheight(math.radians(lat), math.radians(lon), 0)
Exemplo n.º 15
0
def ll(lat, lon):
    return gps.ecef_from_latlongheight(math.radians(lat), math.radians(lon), 0)