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
