def __linewalker(self, startWaypoint, endWaypoint, nudgeDirection=1.5*pi):
"""lineWalker(Waypoint startPoint, Waypoint endPoint, [radians nudgeDirection])
This is the main avoidance logic.
If there are navigation difficulties, it will be replaced by D*.
Slated for redesign pending truck current waypoint information"""
curWaypoint = Waypoint(startWaypoint.getLatitude(), startWaypoint.getLongitude() ,1)
curWaypoint.bumped = False #Dynamically add this attribute to this Waypoint object
remainingDistance = lambda: distance(endWaypoint,curWaypoint)
#If two points are drastically far apart, don't spend the processing time
#to calculate it
if remainingDistance() > self.__MAX_DISTANCE_TO_CALCULATE:
raise InfiniteLoopError, \
"Points are too far apart to navigate between: "+ \
str(remainingDistance())+ " degrees"
proximityThreshold = sqrt(self.__obstacleManager.DEGREES_PER_PIXEL_LONG**2 +
self.__obstacleManager.DEGREES_PER_PIXEL_LAT**2)
dir = direction(startWaypoint, endWaypoint)
#TODO minimum space to clear
while(remainingDistance() > proximityThreshold):
self.__nudgeWaypoint(curWaypoint, dir, 1)
self.__checkAndNudgeWaypoint(curWaypoint, dir+nudgeDirection, 5)
if curWaypoint.bumped: return curWaypoint
return endWaypoint
def update_way_point(self):
try:
self.waypoint = Waypoint(
[self.gps_data_for_out_of_range["latitude"]],
[self.gps_data_for_out_of_range["longitude"]],
)
except TypeError:
print("Error: No gps_data_history but now out of range!")
class connect_request(object):
__slots__ = ["init_pos"]
def __init__(self):
self.init_pos = Waypoint()
def encode(self):
buf = BytesIO()
buf.write(connect_request._get_packed_fingerprint())
self._encode_one(buf)
return buf.getvalue()
def _encode_one(self, buf):
assert self.init_pos._get_packed_fingerprint(
) == Waypoint._get_packed_fingerprint()
self.init_pos._encode_one(buf)
def decode(data):
if hasattr(data, 'read'):
buf = data
else:
buf = BytesIO(data)
if buf.read(8) != connect_request._get_packed_fingerprint():
raise ValueError("Decode error")
return connect_request._decode_one(buf)
decode = staticmethod(decode)
def _decode_one(buf):
self = connect_request()
self.init_pos = Waypoint._decode_one(buf)
return self
_decode_one = staticmethod(_decode_one)
_hash = None
def _get_hash_recursive(parents):
if connect_request in parents: return 0
newparents = parents + [connect_request]
tmphash = (
0x39444e6538533cfc +
Waypoint._get_hash_recursive(newparents)) & 0xffffffffffffffff
tmphash = (((tmphash << 1) & 0xffffffffffffffff) +
((tmphash >> 63) & 0x1)) & 0xffffffffffffffff
return tmphash
_get_hash_recursive = staticmethod(_get_hash_recursive)
_packed_fingerprint = None
def _get_packed_fingerprint():
if connect_request._packed_fingerprint is None:
connect_request._packed_fingerprint = struct.pack(
">Q", connect_request._get_hash_recursive([]))
return connect_request._packed_fingerprint
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
def main():
origin_w = Waypoint(0, 0, 0, 0)
# w1 = Waypoint(-2, 3, 1, 3)
w2 = Waypoint(.2, .3, 2, 6)
w3 = Waypoint(.7, .6, 3, 12)
w4 = Waypoint(1, 1.2, 4, 18)
path = LinearPathGenerator([origin_w, w2, w3, w4])
path.build_path()
trajectory = LinearTrajectoryGenerator(path.get_path(), .5, .05, .2, .025)
trajectory.build_trajectory()
def __init__(self, params):
self.params = params
self.waypoint = Waypoint()
self.mode = 'TEST'
self.speed = 0.0
self.boat_direction = 0.0
self.latitude = 0.0
self.longitude = 0.0
self.timestamp_string = ''
self.target_direction = 0.0
self.target_distance = 0.0
self.gps_data = GpsData()
def __init__(self, params):
self.params = params
self.waypoint = Waypoint()
self.mode = "TEST"
self.speed = 0.0
self.boat_direction = 0.0
self.latitude = 0.0
self.longitude = 0.0
self.timestamp_string = ""
self.target_direction = 0.0
self.target_distance = 0.0
self.gps_data = GpsData()
self.gps_data_for_out_of_range = None
def __init__(self, params):
self.params = params
self.waypoint = Waypoint()
self.mode = "TEST"
self.speed = 0.0
self.boat_heading = 0.0
self.latitude = 0.0
self.longitude = 0.0
self.previous_recorded_latitude = 0.0
self.previous_recorded_longitude = 0.0
self.timestamp_string = ""
self.target_bearing = 0.0
self.target_bearing_relative = 0.0
self.target_distance = 0.0
self.gps_data = GpsData()
self.gps_data_for_out_of_range = None
def _get_hash_recursive(parents):
if action_result in parents: return 0
newparents = parents + [action_result]
tmphash = (0x954ba8e3e4656df + Waypoint._get_hash_recursive(newparents)
) & 0xffffffffffffffff
tmphash = (((tmphash << 1) & 0xffffffffffffffff) +
((tmphash >> 63) & 0x1)) & 0xffffffffffffffff
return tmphash
def _decode_one(buf):
self = action_package()
self.vehicle_id = struct.unpack(">h", buf.read(2))[0]
self.waypoints = []
for i0 in range(10):
self.waypoints.append(Waypoint._decode_one(buf))
self.target_speed = struct.unpack(">d", buf.read(8))[0]
return self
def gotoWaypoint(self, currentWp):
#Before the command was set to send before the waypoint is ready. Was moved to here in the code.
wpCommand = WaypointCommand()
wpCommand.cmd = ">*>ws"
wpCommand.header.stamp = rospy.get_rostime()
self.waypointCmdPublisher.publish(wpCommand)
if currentWp['height'] == None:
wp = Waypoint(currentWp['lng'], currentWp['lat'], self.height, 0,
self.velocity, self.waypointTime)
else:
wp = Waypoint(currentWp['lng'], currentWp['lat'],
currentWp['height'], 0, self.velocity,
self.waypointTime)
self.height = currentWp['height']
wp.getWaypoint(
) #usado para calcular chksum e parametros restantes nao inicializados
waypointData = wp.getWaypointStruct()
waypointData.header.stamp = rospy.get_rostime()
self.waypointDataPublisher.publish(waypointData)
wpCommand = WaypointCommand()
wpCommand.cmd = ">*>wg"
wpCommand.header.stamp = rospy.get_rostime()
self.waypointCmdPublisher.publish(wpCommand)
###################################################################################################
def _get_hash_recursive(parents):
if connect_request in parents: return 0
newparents = parents + [connect_request]
tmphash = (
0x39444e6538533cfc +
Waypoint._get_hash_recursive(newparents)) & 0xffffffffffffffff
tmphash = (((tmphash << 1) & 0xffffffffffffffff) +
((tmphash >> 63) & 0x1)) & 0xffffffffffffffff
return tmphash
def main():
# Take in file location from user
# Create file parser object out of it
# Call parse_file()
# Go through line by line until <coordinate> tag
# Go to next line, set line delimiter to ','
# For each three, create waypoint object and add to array of waypoint objects
# Print leading zeros and then call string function to concat to end of line
print("Testing...")
p1 = Waypoint(1, 2, 3)
print(p1)
waypoint_parser_real = WaypointParser("SampleInput.py")
waypoint_parser_real.parse_file()
return 0
def package(self):
crc = 0
for i in range(0, self.waypoint_count):
data = Waypoint.get_instance(i).package()
crc = crc1(data, len(data), crc)
for i in range(0, self.pathactions):
data = Pathaction.get_instance(i).package()
crc = crc1(data, len(data), crc)
data = []
data.extend(package(self.waypoint_count, 2))
data.extend(package(self.pathactions, 2))
data.extend(package(crc, 1))
return data
def __init__(self,
id,
rooms,
waypoint1,
waypoint2,
open=True,
frame_id="map_align"):
self.id = id
self.rooms = rooms
self.position = [(waypoint1[0] + waypoint2[0]) / 2,
(waypoint1[1] + waypoint2[1]) / 2]
self.waypoint1_pose = waypoint1
self.waypoint2_pose = waypoint2
self.frame_id = frame_id
self.waypoint1 = Waypoint(self.id + "_1", self.waypoint1_pose[0],
self.waypoint1_pose[1],
self.waypoint1_pose[2], self.rooms[0],
self.frame_id)
self.waypoint2 = Waypoint(self.id + "_2", self.waypoint2_pose[0],
self.waypoint2_pose[1],
self.waypoint2_pose[2], self.rooms[1],
self.frame_id)
self.waypoint1_b = Waypoint(self.id + "_1", self.waypoint1_pose[0],
self.waypoint1_pose[1],
self.waypoint1_pose[2] - 3.14,
self.rooms[0], self.frame_id)
self.waypoint2_b = Waypoint(self.id + "_2", self.waypoint2_pose[0],
self.waypoint2_pose[1],
self.waypoint2_pose[2] - 3.14,
self.rooms[1], self.frame_id)
self.status = 2
self.mapped = True
self.mapped_status = int(open)
self.seen = False
self.mapped_time = rospy.Time.now()
self.seen_time = rospy.Time.now()
def parse_file(self):
# Parse through file until reaching '<Waypoints>' tag in .kml
# Check that file actually exists, if not print error and return
if(os.path.exists(self.file_loc) == False):
print("File location doesn't exist bud, try again \n")
return
# Else, file actually exists so lets get started
else:
# Initialize list of waypoint objects and the list for the actual coordinates
waypoints = []
coordinates_list = []
# Open the file and begin to iterate over data, with each waypoint created, write to the output file
input_file = open(self.file_loc, "r")
line = input_file.readline()
while(line):
# For each line in the file...
# If the line contains the coordinates tag...
if("waypoints" in line):
# Get rid of the start and end tags, then set delimiters and create waypoint objects
line = self.format_line(line)
# If the line isnt blank after parsing go ahead and split it and add the coordinates
if(line.split(',') != ['']):
coordinates_list.append(line.split(','))
# Else, this line has nothing in it, meaning the coordinates are actually on the next line
else:
line = input_file.readline()
line = self.format_line(line)
print(line.split(','))
coordinates_list.append(line.split(','))
# Want to go to next line since it is just the closing brackets for coordinate
line = input_file.readline()
line = input_file.readline()
input_file.close()
i = 0
# Assuming we found coordinates within the .kml file...
if(len(coordinates_list) > 0):
while(i < len(coordinates_list)):
# Create a waypoint object with its x y and z components to be added to our waypointsdd list
waypoint = Waypoint(coordinates_list[i][0], coordinates_list[i][1], coordinates_list[i][2])
waypoints.append(waypoint)
i += 1
for waypoint in waypoints:
self.write_file(str(waypoint))
return
def _encode_one(self, buf):
buf.write(struct.pack(">h", self.vehicle_id))
assert self.current_pos._get_packed_fingerprint(
) == Waypoint._get_packed_fingerprint()
self.current_pos._encode_one(buf)
buf.write(struct.pack(">d", self.current_speed))
def gen_waypoint(self) -> Waypoint:
return Waypoint(rd.randrange(0, self.map.size),
rd.randrange(0, self.map.size))
def _get_hash_recursive(parents):
if action_package in parents: return 0
newparents = parents + [action_package]
tmphash = (0x8f3ef868f9459250+ Waypoint._get_hash_recursive(newparents)) & 0xffffffffffffffff
tmphash = (((tmphash<<1)&0xffffffffffffffff) + ((tmphash>>63)&0x1)) & 0xffffffffffffffff
return tmphash
def __init__(self):
self.init_pos = Waypoint()
def _encode_one(self, buf):
buf.write(struct.pack(">h", self.vehicle_id))
for i0 in range(10):
assert self.waypoints[i0]._get_packed_fingerprint() == Waypoint._get_packed_fingerprint()
self.waypoints[i0]._encode_one(buf)
buf.write(struct.pack(">d", self.target_speed))
def _decode_one(buf):
self = action_result()
self.vehicle_id = struct.unpack(">h", buf.read(2))[0]
self.current_pos = Waypoint._decode_one(buf)
self.current_speed = struct.unpack(">d", buf.read(8))[0]
return self
def __init__(self):
self.vehicle_id = 0
self.current_pos = Waypoint()
self.current_speed = 0.0
def _encode_one(self, buf):
assert self.init_pos._get_packed_fingerprint(
) == Waypoint._get_packed_fingerprint()
self.init_pos._encode_one(buf)
def get_waypoints(self,name):
waypoints = rospy.get_param(name)
for i in range(len(waypoints)):
position = waypoints[i].get("pose")
waypoint = Waypoint( waypoints[i].get("name"),position[0],position[1],0.0)
self.waypoints.append(copy.deepcopy(waypoint))
class Status:
def __init__(self, params):
self.params = params
self.waypoint = Waypoint()
self.mode = "TEST"
self.speed = 0.0
self.boat_heading = 0.0
self.latitude = 0.0
self.longitude = 0.0
self.previous_recorded_latitude = 0.0
self.previous_recorded_longitude = 0.0
self.timestamp_string = ""
self.target_bearing = 0.0
self.target_bearing_relative = 0.0
self.target_distance = 0.0
self.gps_data = GpsData()
self.gps_data_for_out_of_range = None
def read_gps(self):
if self.gps_data.read():
lat1, lon1 = self.latitude, self.longitude
lat2, lon2 = (
self.previous_recorded_latitude,
self.previous_recorded_longitude,
)
distance = self._get_distance(lat1, lon1, lat2, lon2)
if distance >= 2: # meter
self.boat_heading = self._get_heading(
self.previous_recorded_latitude,
self.previous_recorded_longitude,
self.latitude,
self.longitude,
)
self.previous_recorded_latitude = self.gps_data.latitude
self.previous_recorded_longitude = self.gps_data.longitude
self.timestamp_string = self.gps_data.timestamp_string
if not self.gps_data_for_out_of_range:
self.gps_data_for_out_of_range = {
"latitude": self.gps_data.latitude,
"longitude": self.gps_data.longitude,
}
self.latitude = self.gps_data.latitude
self.longitude = self.gps_data.longitude
self.speed = self.gps_data.speed[2] # kph
return True
else:
return False
def calc_target_distance(self):
"""
Calculate the distance between the current location and the current waypoint in meters
"""
wp_coordinate = self.waypoint.get_point()
lat1, lon1 = self.latitude, self.longitude
lat2, lon2 = wp_coordinate[0], wp_coordinate[1]
self.target_distance = self._get_distance(lat1, lon1, lat2, lon2)
return
def calc_target_bearing(self):
"""
Calculate the current waypoint's bearing and relative bearing (relative to boat heading) in radians
"""
wp_coordinate = self.waypoint.get_point()
lat1, lon1 = self.latitude, self.longitude
lat2, lon2 = wp_coordinate[0], wp_coordinate[1]
self.target_bearing = self._get_heading(lat1, lon1, lat2, lon2)
self.target_bearing_relative = self.target_bearing - self.boat_heading
return
@staticmethod
def _get_heading(lat1, lon1, lat2, lon2):
"""
Returns boat heading(relative to north pole) in radians
"""
theta1, phi1 = map(math.radians, [lat1, lon1])
theta2, phi2 = map(math.radians, [lat2, lon2])
dphi = phi2 - phi1
y = math.sin(dphi) * math.cos(theta2)
x = math.cos(theta1) * math.sin(theta2) - math.sin(theta1) * math.cos(
theta2) * math.cos(dphi)
dir = math.atan2(y, x)
return dir
@staticmethod
def _get_distance(lat1, lon1, lat2, lon2):
"""
Returns distance between (lon1, lat1) & (lon2, lat2) in meters
"""
r = 6378.137 # [km] # radius of the Earth
theta1, phi1 = map(math.radians, [lat1, lon1])
theta2, phi2 = map(math.radians, [lat2, lon2])
dphi = phi2 - phi1
dtheta = theta2 - theta1
a = (math.sin(dtheta / 2)**2 +
math.cos(theta1) * math.cos(theta2) * math.sin(dphi / 2)**2)
c = 2 * math.asin(math.sqrt(a))
distance = c * r * 1000 # [m]
return distance
def _has_passed_way_point(self):
return self.target_distance < 10.0
def update_target(self):
if self._has_passed_way_point():
# If the boat has passed all waypoints, key is false
# If not, key is true
key = self.waypoint.next_point()
if not key:
print("AN has finished!")
self.mode = "RC"
return
def update_way_point(self):
try:
self.waypoint = Waypoint(
[self.gps_data_for_out_of_range["latitude"]],
[self.gps_data_for_out_of_range["longitude"]],
)
except TypeError:
print("Error: No gps_data_history but now out of range!")
class Status:
def __init__(self, params):
self.params = params
self.waypoint = Waypoint()
self.mode = "TEST"
self.speed = 0.0
self.boat_direction = 0.0
self.latitude = 0.0
self.longitude = 0.0
self.timestamp_string = ""
self.target_direction = 0.0
self.target_distance = 0.0
self.gps_data = GpsData()
self.gps_data_for_out_of_range = None
def readGps(self):
if self.gps_data.read():
diff = abs(self.longitude - self.gps_data.longitude) + abs(
self.latitude - self.gps_data.latitude
)
if diff >= 0.000001:
self.boat_direction = self.getDirection(
self.longitude,
self.latitude,
self.gps_data.longitude,
self.gps_data.latitude,
)
self.timestamp_string = self.gps_data.timestamp_string
if not self.gps_data_for_out_of_range:
self.gps_data_for_out_of_range = {
"latitude": self.gps_data.latitude,
"longitude": self.gps_data.longitude,
}
self.latitude = self.gps_data.latitude
self.longitude = self.gps_data.longitude
self.speed = self.gps_data.speed[2] # kph
return True
else:
return False
# Not Use
def isGpsError(self):
if self.latitude < 0.00001 and self.longitude < 0.00001:
return True
else:
return False
def calcTargetDistance(self):
r = 6378.137 # [km] # radius of the Earth
wp = self.waypoint
lon1 = math.radians(self.longitude)
lon2 = math.radians(wp.getPoint()[1])
lat2 = math.radians(wp.getPoint()[0])
lat1 = math.radians(self.latitude)
dlon = lon2 - lon1
dlat = lat2 - lat1
a = (
math.sin(dlat / 2) ** 2
+ math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2
)
c = 2 * math.asin(math.sqrt(a))
self.target_distance = c * r * 1000 # [m]
return
def calcTargetDirection(self):
wp = self.waypoint
radLonA = math.radians(self.longitude)
radLonB = math.radians(wp.getPoint()[1])
radLatB = math.radians(wp.getPoint()[0])
radLatA = math.radians(self.latitude)
dLong = radLonB - radLonA
y = math.sin(dLong) * math.cos(radLatB)
x = math.cos(radLatA) * math.sin(radLatB) - math.sin(radLatA) * math.cos(
radLatB
) * math.cos(dLong)
dir = math.degrees(math.atan2(y, x))
dir = (dir + 360) % 360
self.target_direction = dir # degrees
return
def getDirection(self, LonA, LatA, LonB, LatB):
radLonA = math.radians(LonA)
radLatA = math.radians(LatA)
radLonB = math.radians(LonB)
radLatB = math.radians(LatB)
dLong = radLonB - radLonA
y = math.sin(dLong) * math.cos(radLatB)
x = math.cos(radLatA) * math.sin(radLatB) - math.sin(radLatA) * math.cos(
radLatB
) * math.cos(dLong)
dir = math.degrees(math.atan2(y, x))
dir = (dir + 360) % 360
return dir
def hasPassedWayPoint(self):
if self.target_distance < 90.0:
return True
else:
return False
def updateTarget(self):
if self.hasPassedWayPoint():
# If the boat has passed all waypoints, key is false
# If not, key is true
key = self.waypoint.nextPoint()
if not key:
print("AN has finished!")
self.mode = "RC"
return
def updateWayPoint(self):
try:
self.waypoint = Waypoint(
[self.gps_data_for_out_of_range["latitude"]],
[self.gps_data_for_out_of_range["longitude"]],
)
except TypeError:
print("Error: No gps_data_history but now out of range!")
def run(self):
self.__log.debug("WaypointThread started")
self.__stopEvent.clear()
userNext = 0
self.__waylist.lock.acquire()
self.__waylist.clear()
truckLocationWaypoint = Waypoint(0,0,3)
truckLocationWaypoint.truck = True
self.__waylist.append(truckLocationWaypoint)
Thread(target = self.__truckUpdater, name = "TruckPositionUpdater", args = (truckLocationWaypoint, self.__TRUCK_UPDATE_PERIOD)).start()
currentWaypoint = None
while not self.__stopEvent.isSet():
with self.__waylist.lock:
truckLocationWaypoint.type = 3
#Allows continuity of the lock from startup to inside the loop.
if currentWaypoint == None:
currentWaypoint = ""
self.__waylist.lock.release()
userWaylist = self.__waylist.userWaypointList
numUserWaypoints = len(userWaylist)
#Note that the truck waypoint above counts as a user waypoint
if numUserWaypoints >= 2:
#Increment
userNext += 1
#Correct for size
userNext %= numUserWaypoints
#Get waypoint object references
currentWaypoint = userWaylist[userNext-1]
nextWaypoint = userWaylist[userNext]
#Navigate
try:
self.__waylist.removeNavigationWaypoints(userNext-1, userNext)
self.__listwalker(self.__waylist.getIndexOfWaypoint(currentWaypoint),
self.__waylist.getIndexOfWaypoint(nextWaypoint),
pi+self.__BUMP_DIRECTION)
except InfiniteLoopError:
try:
self.__waylist.removeNavigationWaypoints(userNext-1, userNext)
self.__listwalker(self.__waylist.getIndexOfWaypoint(currentWaypoint),
self.__waylist.getIndexOfWaypoint(nextWaypoint),
pi-self.__BUMP_DIRECTION)
except InfiniteLoopError:
self.__waylist.removeNavigationWaypoints(userNext-1, userNext)
self.__log.error("Aborted navigation between " +
"user waypoints " +
str(userNext) + ". " +
"Too many intermediate waypoints created.")
self.__vgo.setHaltMode()
self.__vgo.setNavError(True)
truckLocationWaypoint.type = 1
userNext -= 1
self.__pauseEvent.clear()
self.__waylist.lock.release()
self.__pauseEvent.wait()
self.__pauseEvent.clear()
self.__waylist.lock.acquire()
continue
self.__waylist.commit(fromWaypoint = truckLocationWaypoint,
toWaypoint = userWaylist[(userWaylist.index(truckLocationWaypoint)+1)%numUserWaypoints])
truckLocationWaypoint.type = 1
self.__stopEvent.wait(self.__UPDATE_PERIOD_IN_SECONDS)
def _decode_one(buf):
self = connect_request()
self.init_pos = Waypoint._decode_one(buf)
return self
class Status:
def __init__(self, params):
self.params = params
self.waypoint = Waypoint()
self.mode = 'TEST'
self.speed = 0.0
self.boat_direction = 0.0
self.latitude = 0.0
self.longitude = 0.0
self.timestamp_string = ''
self.target_direction = 0.0
self.target_distance = 0.0
self.gps_data = GpsData()
def readGps(self):
if self.gps_data.read():
diff = abs(self.longitude - self.gps_data.longitude) + abs(self.latitude - self.gps_data.latitude)
if diff >= 0.000001:
self.boat_direction = self.getDirection(self.longitude, self.latitude, self.gps_data.longitude, self.gps_data.latitude)
self.timestamp_string = self.gps_data.timestamp_string
self.latitude = self.gps_data.latitude
self.longitude = self.gps_data.longitude
self.speed = self.gps_data.speed[2] #kph
return True
else:
return False
def isGpsError(self):
if self.latitude < 0.00001 and self.longitude < 0.00001:
return True
else:
return False
def calcTargetDistance(self):
r = 6378.137 #[km] # radius of the Earth
wp = self.waypoint
lon1 = math.radians(self.longitude)
lon2 = math.radians(wp.getPoint()[1])
lat2 = math.radians(wp.getPoint()[0])
lat1 = math.radians(self.latitude)
dlon = lon2 - lon1
dlat = lat2 - lat1
a = math.sin(dlat/2)**2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon/2)**2
c = 2 * math.asin(math.sqrt(a))
self.target_distance = c * r * 1000 # [m]
return
def calcTargetDirection(self):
wp = self.waypoint
radLonA = math.radians(self.longitude)
radLonB = math.radians(wp.getPoint()[1])
radLatB = math.radians(wp.getPoint()[0])
radLatA = math.radians(self.latitude)
dLong = radLonB - radLonA
y = math.sin(dLong) * math.cos(radLatB)
x = math.cos(radLatA) * math.sin(radLatB) - math.sin(radLatA) * math.cos(radLatB) * math.cos(dLong)
dir = math.degrees(math.atan2(y, x))
dir = (dir + 360) % 360
self.target_direction = dir # degrees
return
def getDirection(self, LonA, LatA, LonB, LatB):
radLonA = math.radians(LonA)
radLatA = math.radians(LatA)
radLonB = math.radians(LonB)
radLatB = math.radians(LatB)
dLong = radLonB - radLonA
y = math.sin(dLong) * math.cos(radLatB)
x = math.cos(radLatA) * math.sin(radLatB) - math.sin(radLatA) * math.cos(radLatB) * math.cos(dLong)
dir = math.degrees(math.atan2(y, x))
dir = (dir + 360) % 360
return dir
def hasPassedWayPoint(self):
if self.target_distance < 15.0:
return True
else:
return False
def updateTarget(self):
if self.hasPassedWayPoint():
key = self.waypoint.nextPoint()
if not key:
print('AN has finished!')
self.mode = 'RC'
return
class Status:
def __init__(self, params):
self.params = params
self.waypoint = Waypoint()
self.mode = 'TEST'
self.speed = 0.0
self.boat_direction = 0.0
self.latitude = 0.0
self.longitude = 0.0
self.timestamp_string = ''
self.target_direction = 0.0
self.target_distance = 0.0
self.gps_data = GpsData()
def readGps(self):
if self.gps_data.read():
self.timestamp_string = self.gps_data.timestamp_string
self.latitude = self.gps_data.latitude
self.longitude = self.gps_data.longitude
self.speed = self.gps_data.speed[2] #kph
self.boat_direction = self.gps_data.course
return True
else:
return False
def isGpsError(self):
if self.latitude < 0.00001 and self.longitude < 0.00001:
return True
else:
return False
def calcTargetDistance(self):
r = 6378.137 #[km] # radius of the Earth
wp = self.waypoint
y1 = math.radians(self.longitude)
y2 = math.radians(wp.getPoint()[1])
dx = math.radians(wp.getPoint()[0] - self.latitude)
d = r * math.acos(
math.sin(y1) * math.sin(y2) +
math.cos(y1) * math.cos(y2) * math.cos(dx)) # [km]
self.target_distance = d * 1000 # [m]
return
def calcTargetDirection(self):
wp = self.waypoint
y1 = math.radians(self.longitude)
y2 = math.radians(wp.getPoint()[1])
dx = math.radians(wp.getPoint()[0] - self.latitude)
dir = 90 - math.degrees(
math.atan2(
math.cos(y1) * math.tan(y2) - math.sin(y1) * math.cos(dx),
math.sin(dx)))
if dir < 0:
dir = 360 + dir
self.target_direction = dir # degrees
return
def hasPassedWayPoint(self):
if self.target_distance < 1.0:
return True
else:
return False
def updateTarget(self):
if self.hasPassedWayPoint():
key = self.waypoint.nextPoint()
if not key:
print('AN has finished!')
self.mode = 'RC'
return
def main():
pygame.init()
pygame.display.set_caption("abstract algorithm")
# create a surface on screen that has the size of 400 x 300
screen = pygame.display.set_mode((400, 300))
# define a variable to control the main loop
running = True
pause = False
# controlRect
# start Position
x = 30.0
y = 30.0
# plane size
xSize = 60.0
ySize = 60.0
# framerate limiter
clock = pygame.time.Clock()
# initialize plane
plane1 = Plane((0, 128, 255), pygame.Rect(x, y, xSize, ySize))
speed = 3
# initialize queue's
listWaypoints = []
listObstacle = []
# main loop
while running:
# event handling, gets all event from the event queue
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
if pygame.key.get_pressed()[pygame.K_SPACE] == 1:
pause = not pause
pygame.time.wait(250)
if not pause:
# adds waypoints
if pygame.mouse.get_pressed()[0]:
pos = pygame.mouse.get_pos()
newPoint: Waypoint = Waypoint(float(pos[0]), float(pos[1]))
if not (newPoint in listWaypoints):
listWaypoints.append(newPoint)
# adds obstacle
if pygame.mouse.get_pressed()[2]:
pos = pygame.mouse.get_pos()
newPoint: Obstacle = Obstacle(pos[0], pos[1], xSize / 2,
ySize / 2)
if not (newPoint in listObstacle):
listObstacle.append(newPoint)
# detection algorithm
for obstacle in listObstacle:
out = intersect(plane1, obstacle)
obstacle.setColor(colorDict[out])
# movement algorithm
if len(listWaypoints) > 0:
point: Waypoint = listWaypoints[0]
if plane1.getRect().x + xSize + 1.5 > point.xCoordinate > plane1.getRect().x - 1.5 \
and plane1.getRect().y + ySize + 1.5 > point.yCoordinate > plane1.getRect().y - 1.5:
del listWaypoints[0]
else:
plane1.setVelocity(
speed * createUnitVector(point.getCoordinate(),
plane1.getRect().center))
plane1.advancePosition()
screen.fill((0, 0, 0))
pygame.draw.rect(screen, plane1.getColor(), plane1.getRect())
for element in listWaypoints:
pygame.draw.rect(
screen, (128, 128, 128),
pygame.Rect(element.xCoordinate, element.yCoordinate, 10,
10))
for element in listObstacle:
pygame.draw.rect(screen, element.getColor(),
pygame.Rect(element.attribtes()))
if len(listWaypoints) > 0:
pygame.draw.line(
screen, (0, 255, 0),
plane1.getRect().center,
numpy.add(listWaypoints[0].getCoordinate(), [5, 5]))
# refresh screen
pygame.display.flip()
# set framerate
clock.tick(60)
def __init__(self):
self.vehicle_id = 0
self.waypoints = [ Waypoint() for dim0 in range(10) ]
self.target_speed = 0.0
