def add_waypoint_handler(packet_contents): latitude, longitude = packet_contents.data waypoint = constants.Coordinate(latitude, longitude) waypoints.append(waypoint) print("YO im addin a point") tempString = time.strftime("%H%M%S") + " Add Waypoint: lat,lon: " + str( latitude) + "," + str(longitude) Logger.write_line(tempString) print("Added waypoint %s" % (waypoint, ))
def add_waypoint_handler(packet_contents): latitude, longitude = packet_contents.data waypoint = constants.Coordinate(latitude, longitude) waypoints.append(waypoint) print("YO im addin a point") #loggingFile = open(logging, 'a') #log = "Add waypoint lat,lon: " + str(latitude) + "," + str(longitude) + "\n" #loggingFile.write(log) #loggingFile.close() print("Added waypoint %s" % (waypoint, ))
def add_waypoint_handler(packet_contents): latitude, longitude = packet_contents.data waypoint = constants.Coordinate(latitude, longitude) waypoints.append(waypoint) print("YO im addin a point") outFile = open(logging, 'a') tempString = time.strftime("%H%M%S") + " Add Waypoint: lat,lon: " + str( latitude) + "," + str(longitude) + "\n" outFile.write(tempString) outFile.close() print("Added waypoint %s" % (waypoint, ))
def process_gps_data(self, packet): # The GPS sends data as two int32_t's lon, lat = packet.data lat = lat * 1e-7 lon = -lon * 1e-7 timeDifference = time.time() - self._lastTime # print("5") # print(timeDiffernce) # print(time.localtime(time.time())) print(str(time.time())) print(self._location) self._lastTime = time.time() self._location = constants.Coordinate(lat, lon)
def __init__(self, rove_comm, filename): self._pitch = 0 self._roll = 0 self._heading = 0 self._location = constants.Coordinate(0, 0) self._distToGround = 0 self._lidarQuality = 0 # int 5 for brand new data, counts down 1 every 50ms, should never go below 3. self._lastTime = time.time() self.rove_comm_node = rove_comm self.rove_comm_node.subscribe(NAV_IP_ADDRESS) self.rove_comm_node.callbacks[IMU_DATA_ID] = self.process_imu_data self.rove_comm_node.callbacks[GPS_DATA_ID] = self.process_gps_data self.rove_comm_node.callbacks[LIDAR_DATA_ID] = self.process_lidar_data self.loggingFile = filename # see CannyTracking to add logging
def calculate_next_coordinate(start, former_goal): # these need to be mapped to radians for math stuff, otherwise they break in very terrible ways. We might want to just use Haversine here as well? diff_lat = (start.lat - former_goal.lat) * 1000 diff_lon = (start.lon - former_goal.lon) * 1000 r = math.sqrt(diff_lat ** 2 + diff_lon ** 2) theta = r / constants.SEARCH_DISTANCE # Add delta theta to calculate new point with theta += constants.DELTA_THETA # We should definitely decrease theta from math.pi/2 to pi/4, pi/6 or even pi/8 # needs additional scaling at some point to handle large search patterns appropriately. r = constants.SEARCH_DISTANCE * theta diff_lat = r * math.sin(theta) diff_lon = r * math.cos(theta) return constants.Coordinate(start.lat + diff_lat/1000, start.lon + diff_lon/1000)
rovecomm_node.write(drive.send_drive(0, 0)) # rovecomm_node.write(RoveCommPacket(1000, 'h', (0,0), ip_octet_4=140)) r = 6371 # radius of earth brng = math.radians(nav_board._heading - 90) # target heading d = 0.0005 # 5 meters lat1 = math.radians(nav_board._location[0]) lon1 = math.radians(nav_board._location[1]) lat2 = math.asin( math.sin(lat1) * math.cos(d / r) + math.cos(lat1) * math.sin(d / r) * math.sin(brng)) lon2 = lon1 + math.atan2( math.sin(brng) * math.sin(d / r) * math.cos(lat1), math.cos(d / r) - math.sin(lat1) * math.sin(lat2)) lat2 = math.degrees(lat2) lon2 = math.degrees(lon2) target = constants.Coordinate(lat2, lon2) print(target) left, right = gps_nav.calculate_move(target, nav_board.location(), start, drive, nav_board) if nav_board._distToGround > constants.LIDAR_MAXIMUM: rovecomm_node.write(drive.send_drive(0, 0)) continue rovecomm_node.write(drive.send_drive(left, right)) distance = 10 while distance > gps_nav.WAYPOINT_DISTANCE_THRESHOLD: time.sleep(0.1) if state_switcher.state != rs.ObstacleAvoidance(): rovecomm_node.write(drive.send_drive(0, 0)) continue left, right = gps_nav.calculate_move(target, nav_board.location(), start, drive, nav_board)
def add_waypoint_handler(packet_contents): latitude, longitude = packet_contents.data waypoint = constants.Coordinate(latitude, longitude) waypoints.append(waypoint) print("Added waypoint %s" % (waypoint,))
import logging import time import constants import rover_states import algorithms.marker_search as marker_search import algorithms.gps_navigate as gps_nav state_switcher = rover_states.StateSwitcher() state_switcher.state = rover_states.Searching() gps_data = gps_nav.GPSData() gps_data.start = constants.Coordinate(37.950271, -91.777770) current_loc = gps_data.start gps_data.goal = marker_search.calculate_next_coordinate( gps_data.start, current_loc) print("Origin: " + str(gps_data.start.lat) + ", " + str(gps_data.start.lon)) while True: if state_switcher.state == rover_states.Searching(): print() goal, start = gps_data.data() #print("Current: " + str(current_loc)) #print("Goal: " + str(goal)) new_goal = marker_search.calculate_next_coordinate(start, current_loc) print("New Goal: " + str(new_goal.lat) + ", " + str(new_goal.lon)) current_loc = goal