def _calculate_separation(self, boats):
'''
Function to calculate separation vector
Args:
boats: list of dictionaries containing data from other boats
Returns:
A vector containing separation for unit
'''
separation = Vector()
total = 0.0
average_vector = Vector()
for boid in boats:
if boid['distance'] < self.perception and boid['distance'] != 0.0: # only boids within perception calculated
diff = Vector(-boid['x'], -boid['y'])
diff = diff.__truediv__(boid['distance'])
average_vector += diff
total += 1.0
if total > 0.0 and average_vector.magnitude != 0.0 and average_vector.angle != 0.0:
separation = average_vector.__truediv__(total)
separation_tot = m.sqrt(m.pow(separation.magnitude, 2.0)+m.pow(separation.angle, 2.0))
if separation_tot > 0.0: #Proportions the vector to maxSpeed
separation = (separation.__truediv__(separation_tot)) * self.maxSpeed
if separation_tot > self.maxForce:
separation = (separation.__truediv__(separation_tot)) * self.maxForce
return separation
class swarmWanted():
'''Helper class to fetch data from behaviour through ROS'''
def __init__(self):
'''Initialises subscribers to behaviour topics'''
self.topic_main = "/swarm/behaviour/"
topic_movement = "/swarm/behaviour/movement"
topic_position = "/swarm/behaviour/position"
try:
rospy.Subscriber(topic_movement, Movement, self._update_movement)
rospy.Subscriber(topic_position, Position, self._update_position)
except rospy.exceptions.ROSException as e:
print("could not subscribe to topic with error: {s}", format(e))
self.swarm_movement = Vector()
self.swarm_position = GPS()
self.time_since = 0
self.last_receive = rospy.get_rostime().secs
self.newest = ''
self.recieving_data = False
def __call__(self):
'''Call function that returns newest data
Returns either:
Vector of newest movement wanted
GPS point wanted
'''
if self.newest == 'movement':
return self.swarm_movement
else:
return self.swarm_position
def _update_movement(self, data):
self.swarm_movement.set(data.velocity, data.bearing)
self.last_receive = rospy.get_rostime().secs
self.newest = 'movement'
def _update_position(self, data):
self.swarm_position.set(data.latitude, data.longitude)
self.last_receive = rospy.get_rostime().secs
self.newest = 'position'
def is_recieving(self):
'''Helper function to return if topics are published to
Returns:
Boolean on wether its receiving or not
'''
self.time_since = rospy.get_rostime().secs - self.last_receive
if self.time_since < 2 and self.newest != '':
self.recieving_data = True
return self.recieving_data
def get_Vector(self):
'''Helper function get current movement vector
Returns:
Vector object containing current movement
'''
self.vector = Vector(self.velocity, self.bearing)
return self.vector
def _calculate_cohesion(self, boats):
'''
Function to calculate cohesion vector
Args:
boats: list of dictionaries containing data from other boats
Returns:
A vector containing cohesion for unit
'''
cohesion = Vector()
total = 0.0
center_of_mass = Vector()
for boid in boats:
if boid['distance'] < self.perception and boid['distance'] != 0.0: # only boids within perception calculated
center_of_mass.magnitude += boid['x']
center_of_mass.angle += boid['y']
total += 1.0
if total > 0.0 and center_of_mass.magnitude != 0.0 and center_of_mass.angle != 0.0: #only manipulates vector if there is one
cohesion = center_of_mass.__truediv__(total) #- self.position
cohesion_tot = m.sqrt(m.pow(cohesion.magnitude, 2.0)+m.pow(cohesion.angle, 2.0))
if cohesion_tot > 0.0: #Proportions the vector to maxSpeed
cohesion = (cohesion.__truediv__(cohesion_tot)) * self.maxSpeed
if cohesion_tot > self.maxForce:
cohesion = (cohesion.__truediv__(cohesion_tot)) * self.maxForce
return cohesion # vector towards center of mass - cohesion
def _calculate_alignment(self, boats):
'''
Function to calculate alignment vector
Args:
boats: list of dictionaries containing data from other boats
Returns:
A vector containing alignment for unit
'''
alignment = Vector()
total = 0.0
average_temp = Vector(0.0, 0.0)
average_vector = Vector(0.0, 0.0)
for boid in boats:
if boid['distance'] < self.perception and boid['distance'] != 0.0: #finds number of boids within perception
dx = boid['speed'] * m.sin(m.radians(boid['bearing'])) #converts vector to x,y components
dy = boid['speed'] * m.cos(m.radians(boid['bearing']))
average_temp.set(dx,dy)
average_vector += average_temp
total += 1.0
if total > 0.0:
try:
alignment.magnitude = average_vector.magnitude / total
except ValueError:
pass
try:
alignment.angle = average_vector.angle / total
except ValueError:
pass
alignment_tot = m.sqrt(m.pow(average_vector.magnitude, 2.0) + m.pow(average_vector.angle, 2.0))
if alignment_tot > self.maxForce:
alignment = (alignment.__truediv__(alignment_tot)) * self.maxForce
return alignment
def __init__(self):
'''Initialises subscribers to behaviour topics'''
self.topic_main = "/swarm/behaviour/"
topic_movement = "/swarm/behaviour/movement"
topic_position = "/swarm/behaviour/position"
try:
rospy.Subscriber(topic_movement, Movement, self._update_movement)
rospy.Subscriber(topic_position, Position, self._update_position)
except rospy.exceptions.ROSException as e:
print("could not subscribe to topic with error: {s}", format(e))
self.swarm_movement = Vector()
self.swarm_position = GPS()
self.time_since = 0
self.last_receive = rospy.get_rostime().secs
self.newest = ''
self.recieving_data = False
def __init__(self):
self.Ka = 1.5 #Adjusting alignment
self.Kc = 1.85 #Adjusting cohesion
self.Ks = 0.5 #Adjusting separation
self.maxForce = 1.2 # maximum magnitude of cohesion and separation
self.maxSpeed = 2.0 # Maximum speed in m/s
self.perception = 100.0 # Max distance to percieve other boats
self.position = GPS()
self.movement = Vector()
self.has_newCurr = False
def main():
nav = navData()
autopilot = Autopilot()
time.sleep(0.2)
autopilot_talker = Talker()
time.sleep(0.2)
wait_time, clicks = 0.0, 0
wanted_GPS = GPS(60.365625, 5.264544)
wanted_vector = Vector(0.4, 180.0)
while nav.is_ready() == False:
#waits for both systems to connect
wait_time += 0.1
time.sleep(0.1)
if wait_time > 10.0: #exit if timeout is over 10s
sys.exit(0)
if nav.is_ready() == True:
print("Pixhawk is connected and ready at: ", nav.mode)
print("entering autopilot loop...")
while not rospy.is_shutdown():
try:
# wanted_GPS = new_gps(wanted_GPS)
# fetches newest current speed and position
current_GPS = nav.get_GPS()
current_vector = nav.get_Vector()
autopilot.set_wanted_vector(wanted_vector)
change_vector = autopilot(current_vector)
#publishing to ROS
autopilot_talker(current_vector, current_GPS, wanted_vector,
change_vector)
clicks += 1
time.sleep(0.2)
except rospy.ROSInterruptException():
sys.exit()
finally:
pass
def run(self) -> None:
global functions_sequence
self.run_available = True
while self.run_available:
sleep(self.sleep_time)
if len(functions_sequence[0]) > 0:
function = functions_sequence[0].pop(0)
arguments = functions_sequence[1].pop(0)
key_word_arguments = functions_sequence[2].pop(0)
function(*arguments, **key_word_arguments)
if self.serial is not None:
if not self.serial.is_open:
self.serial.open()
while self.serial.in_waiting:
try:
data: bytes = self.serial.readline()
if type(data) == bytes:
data: str = data.decode('utf-8').strip()
if data != "":
data = data.replace('$', '')
data = data.replace(';', '')
data = data.strip()
parse = data.split()
parse = {
"+x":
self.plus_x_filter.latest_noisy_measurement(
float(parse[0][2:])) - self.plus_x_cof,
"-x":
self.minus_x_filter.latest_noisy_measurement(
float(parse[1][2:])) - self.minus_x_cof,
"+y":
self.plus_y_filter.latest_noisy_measurement(
float(parse[2][2:])) - self.plus_y_cof,
"-y":
self.minus_y_filter.latest_noisy_measurement(
float(parse[3][2:])) - self.minus_y_cof
}
vec1 = Vector((0, 0, 0),
(-parse['-x'], 0, parse['-x']))
vec2 = Vector((0, 0, 0),
(parse['+x'], 0, parse['+x']))
vec3 = Vector((0, 0, 0),
(0, -parse['-y'], parse['-y']))
vec4 = Vector((0, 0, 0),
(0, parse['+y'], parse['+y']))
sum_vec = vec1 + vec2 + vec3 + vec4
sum_vec.set_length(
abs(parse['-x']) + abs(parse['+x']) +
abs(parse['-y']) + abs(parse['+y']))
if len(self.buffer) >= self.data_buffer_len:
self.writing_in_buffer_flag = True
self.buffer.append((sum_vec, parse))
self.buffer.pop(0)
self.writing_in_buffer_flag = False
else:
self.writing_in_buffer_flag = True
self.buffer.append((sum_vec, parse))
self.writing_in_buffer_flag = False
if self.graph is not None:
self.graph.add_to_buffer((sum_vec, parse))
except:
pass
#!/usr/bin/env python import time from Classes.PID import PID from Classes.GPS_class import GPS from Classes.Ranger import autoRange from Classes.Vector_class import Vector v1 = Vector(10.0,190.0) v2 = Vector(10.0,190.0) g1 = GPS(60.366468, 5.258200) g2 = GPS(60.366455, 5.258232) v3 = g1.calculate(g2) v3.showVector() pid = PID() pid.set_wanted(v1) pid_out = pid.update(v2) pid_out.showVector() rangespeed = autoRange(0.0,20.0,0.0,90.0) rangeangle = autoRange(-45.0,45.0,0.0,180.0)
def main():
#definitin of status dictionary
status = {'pixhawk': False, 'arduino': False, 'fix': False, 'wifi': False}
#initiating objects for autopilot
nav = navData()
autopilot = Autopilot()
time.sleep(0.2)
talker = Talker() # change between for sim or real
# sim = Sim()
time.sleep(0.2)
arduino = Arduino('/dev/Arduino',
speedLimit=0.8) #speed limiter for testing
time.sleep(0.2)
behaviour = swarmWanted()
time.sleep(0.2)
wait_time, clicks = 0.0, 0
#waits for both systems to connect
while not nav.is_ready() and not arduino.is_ready():
wait_time += 0.1
time.sleep(0.1)
if wait_time > 10.0: #exit if timeout is over 10s
sys.exit(0)
rospy.loginfo("Pixhawk is connected and ready: {}".format(nav.mode))
rospy.loginfo("Arduino is connected and started at: {}".format(
arduino.port))
rospy.loginfo("Behaviour is publishing data at: {}".format(
behaviour.topic_main))
status = {'pixhawk': True, 'arduino': True, 'fix': False, 'wifi': False}
print("entering loop...")
while not rospy.is_shutdown():
try:
current_GPS = nav.get_GPS() #gets newest
current_vector = nav.get_Vector()
if behaviour.is_recieving(
): # check if behaviour is sending wanted
wanted = behaviour()
if isinstance(wanted, GPS):
wanted_vector = current_GPS.calculate(wanted)
else:
wanted_vector = wanted
else: #if not recieving from behaviour stop USV
print("did not recieve")
wanted_vector = Vector(0.0, 0.0)
#sets wanted value for regulator
autopilot.set_wanted_vector(wanted_vector)
#calculates vector for new mvoement
change_vector = autopilot(current_vector)
#sends vector with new movmement to arduino
arduino(change_vector.magnitude,
change_vector.angle) #possible addition
#publishes current data to ROS
talker(current_vector, current_GPS, wanted_vector,
change_vector) #change between for sim or real
# sim()
#publishes status of USV every 20 clicks
if clicks >= 20:
talker.publish_status(status)
clicks = 0
else:
clicks += 1
#comment out for full test
time.sleep(0.5)
except rospy.ROSInterruptException():
arduino()
sys.exit()
finally:
pass
arduino() # to reset boat when exiting node
if ids is not None:
ids_dict = {}
ids_middle = {}
for i in range(ids.size):
ids_dict[int(ids[i][0])] = corners[i][0]
ids_middle[int(ids[i][0])] = find_middle(corners[i][0])
strg = ''
for i in range(0, ids.size):
strg += str(ids[i][0]) + ', '
_id = ids[i][0]
if _id in zero_ids:
if _id == 0:
mid_zero = ids_middle[ids[i][0]]
ADAPTIVE_CONSTANT = Zero_marker0.size / \
Vector(ids_dict[ids[i][0]][0], ids_dict[ids[i][0]][1]).length()
move_vector = Vector(mid_zero, frame_center)
if move_vector.length() > 2:
move_vector.set_length(move_vector.length() * ADAPTIVE_CONSTANT)
manipulator.move_by_vector(move_vector)
if mid_zero is not None:
if _id == 1:
if ids_middle.get(0, False):
x_vector = Vector(ids_middle[0], ids_middle[1])
x_vector_move = x_vector.copy()
x_vector_move.set_length(x_vector.length() * ADAPTIVE_CONSTANT)
if _id == 2:
if ids_middle.get(0, False):
y_vector = Vector(ids_middle[0], ids_middle[2])
y_vector_move = y_vector.copy()
if ids is not None:
ids_dict = {}
ids_middle = {}
for i in range(ids.size):
ids_dict[int(ids[i][0])] = corners[i][0]
ids_middle[int(ids[i][0])] = find_middle(corners[i][0])
strg = ''
for i in range(0, ids.size):
strg += str(ids[i][0]) + ', '
_id = ids[i][0]
if _id in zero_ids:
if _id == 0:
mid_zero = ids_middle[ids[i][0]]
ADAPTIVE_CONSTANT = Zero_marker0.size / \
Vector(ids_dict[ids[i][0]][0], ids_dict[ids[i][0]][1]).length()
move_vector = Vector(mid_zero, frame_center)
if move_vector.length() > 2:
move_vector.set_length(move_vector.length() *
ADAPTIVE_CONSTANT)
manipulator.move_by_vector(move_vector)
if mid_zero is not None:
if _id == 1:
if ids_middle.get(0, False):
x_vector = Vector(ids_middle[0], ids_middle[1])
x_vector_move = x_vector.copy()
x_vector_move.set_length(x_vector.length() *
ADAPTIVE_CONSTANT)
if _id == 2:
if ids_middle.get(0, False):