def __init__(self):
'''Initialises subscribers to active given topics'''
rospy.init_node('navigation', anonymous=True)
statePX = "/mavros/state"
GPSPX = "/mavros/global_position/raw/fix"
compassPX = "/mavros/vfr_hud"
velocityPX = "/mavros/global_position/raw/gps_vel"
rospy.Subscriber(statePX, mavros_msgs.msg.State, self._handle_state)
rospy.Subscriber(GPSPX, sensor_msgs.msg.NavSatFix, self._handle_GPS)
rospy.Subscriber(compassPX, mavros_msgs.msg.VFR_HUD, self._handle_compass)
rospy.Subscriber(velocityPX, geometry_msgs.msg.TwistStamped, self._handle_velocity)
self.is_connected = False
self.has_GPS = False
self.has_bearing = False
self.has_velocity = False
self.connection = False
self.mode = ""
self.lat = 0.0
self.lon = 0.0
self.GPS = GPS()
self.bearing = 0.0
self.velocity = 0.0
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 __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
class navData:
'''Helper class that fetches data from sensor (PX4) through ROS topics'''
def __init__(self):
'''Initialises subscribers to active given topics'''
rospy.init_node('navigation', anonymous=True)
statePX = "/mavros/state"
GPSPX = "/mavros/global_position/raw/fix"
compassPX = "/mavros/vfr_hud"
velocityPX = "/mavros/global_position/raw/gps_vel"
rospy.Subscriber(statePX, mavros_msgs.msg.State, self._handle_state)
rospy.Subscriber(GPSPX, sensor_msgs.msg.NavSatFix, self._handle_GPS)
rospy.Subscriber(compassPX, mavros_msgs.msg.VFR_HUD, self._handle_compass)
rospy.Subscriber(velocityPX, geometry_msgs.msg.TwistStamped, self._handle_velocity)
self.is_connected = False
self.has_GPS = False
self.has_bearing = False
self.has_velocity = False
self.connection = False
self.mode = ""
self.lat = 0.0
self.lon = 0.0
self.GPS = GPS()
self.bearing = 0.0
self.velocity = 0.0
def _handle_state(self, msg):
self.connection = msg.connected
self.mode = msg.mode
self.is_connected = True
def _handle_GPS(self, msg):
self.lat = msg.latitude
self.lon = msg.longitude
self.GPS.set(self.lat, self.lon)
self.has_GPS = True
def _handle_compass(self, msg):
self.bearing = msg.heading
self.has_bearing = True
def _handle_velocity(self, msg):
velEast = msg.twist.linear.x
velNorth = msg.twist.linear.y
self.velocity = m.sqrt((velEast)**2+(velNorth)**2)
self.has_velocity = True
def get_connection_state(self):
'''Helper function get connection state of sensor
Returns:
Boolean to determine conneciton state
'''
return self.connection
def get_GPS(self):
'''Helper function get current GPS position
Returns:
GPS point to current position
'''
return self.GPS
def get_bearing(self):
'''Helper function get current bearing
Returns:
Float of current bearing
'''
return self.bearing
def get_velocity(self):
'''Helper function get current velocity
Returns:
Float of current velocity
'''
return self.velocity
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 is_ready(self):
'''Helper function to determine if all sensors are ready and publishing
Returns:
Boolean of wether sensors are ready or not
'''
if all( [self.is_connected == True,
self.has_GPS == True,
self.has_bearing == True,
self.has_velocity == True] ):
return True
else:
return False
#!/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)