def __init__(self):
rospy.init_node('preserve_height', anonymous=True)
self.armDisarmService = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.RcOverrideTopic = rospy.Publisher('/mavros/rc/override',
OverrideRCIn,
queue_size=10)
self.RcMessage = OverrideRCIn()
def begin_mission(self):
self.mode[0] = 'auto'
self.srv_mode[0](0, '3')
rc_msg = OverrideRCIn()
(rc_msg.channels[0], rc_msg.channels[1], rc_msg.channels[2],
rc_msg.channels[3], rc_msg.channels[4]) = (1500, 1500, 1250, 1500,
1400)
self.pub_rc[0].publish(rc_msg)
print "Beginning mission"
def Disarm(self):
self.RcMessage = OverrideRCIn()
for i in range(0, 7):
self.RcMessage.channels[i] = OverrideRCIn.CHAN_NOCHANGE
self.RcMessage.channels[
2] = 1000 #Set throttle to low speed so we can arm
self.RcOverrideTopic.publish(self.RcMessage)
time.sleep(1)
self.armDisarmService(0)
def __init__(self, height, alt_hold_time):
self.overridePub = rospy.Publisher('/mavros/rc/override',
OverrideRCIn,
queue_size=10)
self.armDisarmService = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.height = height
self.reached = False
self.alt_hold_time = alt_hold_time
self.msg = OverrideRCIn()
dir(self.msg)
def fly(self):
if self.buttons:
# Arm drone
if self.buttons[2]:
self.srv_mode(0, '5')
self.srv_arm(True)
print "Arming drone"
# Disarm drone
if self.buttons[3]:
self.srv_arm(False)
print "Disarming drone"
if self.drone.armed:
rc_msg = OverrideRCIn()
if abs(self.axes[0]) > 0.00001 or abs(self.axes[1]) > 0.00001:
x = 1500 - self.axes[0] * 300
y = 1500 - self.axes[1] * 300
yaw = 1500 - self.axes[2] * 200
else:
scale_x = self.fiducial[0]
scale_y = self.fiducial[1]
x = 1500 + scale_x * 50
y = 1500 - scale_y * 50
yaw = 1500
if self.axes[3] > 0.8:
z = 1600
elif self.axes[3] < -0.8:
z = 1400
else:
z = 1500
(rc_msg.channels[0], rc_msg.channels[1], rc_msg.channels[2],
rc_msg.channels[3], rc_msg.channels[6]) = (x, y, z, yaw, 1250)
self.pub_rc.publish(rc_msg)
def fly(self):
if self.buttons:
#RTL
if self.buttons[0]:
self.RTL()
# Arm drone(s)
if self.buttons[2]:
self.arm()
# Disarm drone(s)
if self.buttons[3]:
self.disarm()
#set mission
if self.buttons[10]:
self.waypoints = mission_parser.get_mission(0)
#self.continue_mission() #keep drone from landing at end of mission
success = self.push_waypoints() #push to drone
if self.buttons[11]:
self.clear_waypoints()
'''
#set new waypoints - in flight, ignore existing waypoints and start new mission
if self.buttons[9]:
self.waypoints = mission_parser.get_mission(1)
#self.continue_mission()
self.push_waypoints()
self.restart_mission()
'''
#save map points to mission
if self.buttons[9]:
self.set_mission_to_map()
'''
#add waypoints
if self.buttons[8]:
self.add_waypoints([[42.293394, -71.263916],[42.293357, -71.263997]])
'''
if self.buttons[8]:
self.save_mission_to_file()
#enter guided mode/toggle guided waypoints
if self.buttons[1]:
self.guided_function()
if self.buttons[4]:
self.begin_mission()
if self.buttons[5]:
self.end_mission()
if self.buttons[6]:
self.set_guided_to_map()
if self.buttons[7]:
self.launch_map()
time.sleep(0.01)
#joystick control for manual
if self.drones[0].armed and self.mode[0] == 'manual':
rc_msg = OverrideRCIn()
x = 1500 - self.axes[0] * 300
y = 1500 - self.axes[1] * 300
z = 1000 + (self.axes[3] + 1) * 500
yaw = 1500 - self.axes[2] * 200
(rc_msg.channels[0], rc_msg.channels[1], rc_msg.channels[2],
rc_msg.channels[3]) = (x, y, z, yaw)
print z
self.pub_rc[0].publish(rc_msg)
def fly(self):
if self.buttons:
# Button 1 - enters failsafe mode (enables full control)
if self.buttons[0]:
self.failsafe = not self.failsafe
# Button 3 - arms the drone
if self.buttons[2]:
self.srv_arm(True)
print "Arming drone"
# Button 4 - disarms drone
if self.buttons[3]:
self.srv_arm(False)
print "Disarming drone"
# Button 5 - initiate autonomy routine
if self.buttons[4]:
self.drone.mode = 1
self.time_mode_started = millis()
print "Beginning finite state machine"
# Button 6 - end autonomy routine (RTL)
if self.buttons[5]:
self.drone.mode = 5
print "Returning to launch and landing"
# Initiates finite state machines
if not self.failsafe:
# Arming
if self.drone.mode == 1:
self.srv_mode(0, '5')
self.arm()
# Launching
if self.drone.mode == 2:
self.launch()
# Tracking
if self.drone.mode == 3:
self.track()
# Returning to launch
if self.drone.mode == 4:
self.rtl()
# Landing
if self.drone.mode == 5:
self.land()
# Disarming
if self.drone.mode == 6:
self.disarm()
else:
# Reads joystick values
self.srv_mode(0, '5')
self.drone.mode = 0
x = 1500 - self.axes[0] * 300
y = 1500 - self.axes[1] * 300
z = 2000 + self.axes[3] * 1000
yaw = 1500 - self.axes[2] * 300
(self.drone.x, self.drone.y, self.drone.z, self.drone.yaw) = (x, y, z, yaw)
# Publishes commands
if self.drone.armed:
rc_msg = OverrideRCIn()
rc_msg.channels = [self.drone.x, self.drone.y, self.drone.z, self.drone.yaw, 1400, 0, 0, self.drone.cam_tilt]
self.pub_rc.publish(rc_msg)
self.sub_mode.publish(self.drone.mode)
def GetRcMessage(self):
msg = OverrideRCIn()
msg.channels = self.__rcChannels
return msg
def __init__(self):
self.__rcChannels = OverrideRCIn().channels
