def safeRCout(self):
#rospy.loginfo('mag=%s, dir=%s', self.magnitude, self.direction)
if self.magnitude is None or self.direction is None or self.buttons is None:
return
RCout = OverrideRCIn()
rad=math.radians(self.direction)
r= math.cos(rad) * self.magnitude/2 + 1500
p= -1*math.sin(rad) * self.magnitude/2 + 1500
t= self.axes[1]
y= -1*self.axes[0]
t=interp(t,[-1,1],[1300,1700])
y=interp(y,[-1,1],[1300,1700])
if r>1850:
r=1850
elif r<1150:
r=1150
if p>1850:
p=1850
elif p<1150:
p=1150
if self.rtrig ==1:
RCout.channels = numpy.array([ r , p , t , y , 0 , 0 , 0 , 0 ],dtype=numpy.uint16)
else:
RCout.channels = numpy.array([ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ],dtype=numpy.uint16)
#rospy.loginfo('x=%s, y=%s rad=%s magnitude=%s RCout=%s direction=%s', x, y , rad, self.magnitude, RCout.channels , self.direction )
self.pub.publish(RCout)
def callback(msg):
rcinput = msg.channels
x = rcinput[0] - 1500
y = (rcinput[1] - 1500)*-1
direction=math.atan2(y,x)*180/math.pi
magnitude=math.sqrt(x**2+y**2)
if direction<0:
direction = direction+360
rad=math.radians(direction)
x=math.cos(rad) * magnitude + 1500
y=math.sin(rad) * magnitude + 1500
RCoveride = OverrideRCIn()
if x>1850:
x=1850.0
elif x<1150:
x=1150.0
if y>1850:
y=1850.0
elif y<1150:
y=1150.0
RCoveride.channels = numpy.array( [x,y,0,0,0,0,0,0], dtype=numpy.uint16)
rospy.loginfo( 'RCoveride= %s', RCoveride)
pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=1)
pub.publish(RCoveride)
def handler_rc(data):
# global last_time
cmd = OverrideRCIn()
cmd.channels = data.channels
cmd_publisher.publish( cmd )
def init_mission(self, mission = []):
rospy.loginfo("Initializing...")
# First WAIT
self.mode = Droid.MODE.wait
init_mission = []
ready = (self.state.armed and self.rel_alt > 0)
# Resquest mode LOITER
self.change_mode( mav_mode = Droid.MAV_MODE.loiter )
# rospy.sleep(1)
if not ready:
# Request to ARM the Quad
self.arm()
rospy.sleep(2)
# Create the Initial mission
# With the home position and take off waypoint
lat, lon = self.gps.latitude, self.gps.longitude
rospy.loginfo("Create start mission")
wp_home = utils.create_waypoint( x=lat, y=lon )
wp = utils.create_waypoint( x=lat, y=lon, z=DroidCopter.MISSION_ALTITUDE, wp_type = Waypoint.NAV_TAKEOFF )
init_mission.extend( [wp_home, wp] )
# Send the mission
init_mission.extend( mission )
res = self.service_mav_mission_push( init_mission )
utils.log_success( res.success, "Mission sended" )
rospy.sleep(2)
# Request mode AUTO
self.change_mode( mav_mode = Droid.MAV_MODE.auto )
if not ready:
# Send RC commands to init the AUTO mission
cmd = OverrideRCIn()
cmd.channels = [ cmd.CHAN_NOCHANGE ] * 8
cmd.channels[2] = 1700
self.topic_rc_pub.publish( cmd )
rospy.sleep(2)
# Release RC commands
cmd.channels[2] = cmd.CHAN_RELEASE
self.topic_rc_pub.publish( cmd )
rospy.sleep(2)
# Wait until reach the desired inital altitude
if not mission:
while self.rel_alt < DroidCopter.MISSION_ALTITUDE and not rospy.is_shutdown():
rospy.sleep(1)
def Control_To_RC(channel):
data=OverrideRCIn()
#Release all channels back to RC
command=[RELEASE,RELEASE,RELEASE,RELEASE,RELEASE,RELEASE,RELEASE,RELEASE]
data.channels=command
channel.publish(data)
rospy.logwarn('['+NODE_NAME+']: Returning control to RC')
return
def callback(msg):
rcinput=msg.channels
RCoveride=OverrideRCIn()
RCoveride.channels = numpy.array(rcinput, dtype=numpy.uint16)
#RCoveride.channels = numpy.array([0,0,0,0,0,0,0,0], dtype=numpy.uint32)
rospy.loginfo( 'RCoveride= %s', RCoveride)
pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=1)
pub.publish(RCoveride)
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 handler_rc(data):
cmd = OverrideRCIn()
# cmd.channels = data.channels
linear = data.linear.x
angular = data.angular.z
if linear >= 0:
linear = int( interp( linear, [0, 1], [ MID_LINEAR, MAX_LINEAR ] ) )
else:
linear = int( interp( abs( linear ), [0, 1], [ MID_LINEAR, MIN_LINEAR ] ) )
angular = int( interp( angular, [-1, 1], [ MAX_ANGLE, MIN_ANGLE ] ) )
cmd.channels[0] = angular # angular
cmd.channels[2] = linear # linear
cmd_publisher.publish( cmd )
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 Run_Blender(self):
loop_rate=rospy.Rate(30)
instr=Instruction()
current_point=Point()
target_point=Point()
my_id = sml_setup.Get_Parameter(NODE_NAME,'body_id',1)
bodies = sml_setup.Get_Parameter(NODE_NAME,'body_array',[1,2])
#Publish to RC Override
rc_override=rospy.Publisher('mavros/rc/override',OverrideRCIn,queue_size=10)
self.init_subscriptions(target_point, current_point, instr)
data_init=OverrideRCIn()
command=[0,0,self.CONTROL_ARMING_MIN,0,0,0,0,0]
data_init.channels=command
#Wait until the security guard is online
self.Wait_For_Security_Guard(instr)
#integral term initialized to 0
self.PID.set_d_updated(0)
while not rospy.is_shutdown():
if not target_point.first_point_received:
self.Wait_For_First_Point(target_point,rc_override,data_init,loop_rate)
#reinitialize d_updated
self.PID.set_d_updated(0)
d = self.PID.get_d_updated()
x,x_vel,x_acc=self.PID.Get_Pos_Vel_Acc(current_point)
x_target,x_vel_target,x_acc_target=self.PID.Get_Pos_Vel_Acc(target_point)
u = self.blend(current_point, target_point,d)
command_controlled = self.get_controloutput(u,x,x_target)
#If OK from security guard, publish the messages via Mavros to the drone
if instr.permission:
data=OverrideRCIn()
data.channels=command_controlled
rc_override.publish(data)
loop_rate.sleep()
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 callback(self,data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
height, width, channels = cv_image.shape
crop_img = cv_image[200:(height)/2+150][1:width]
lower = np.array([0, 0, 79], dtype = "uint8")
upper = np.array([40, 40, 191], dtype = "uint8")
mask = cv2.inRange(crop_img, lower, upper)
extraction = cv2.bitwise_and(crop_img, crop_img, mask = mask)
m = cv2.moments(mask, False)
try:
x, y = m['m10']/m['m00'], m['m01']/m['m00']
except ZeroDivisionError:
x, y = height/2, width/2
cv2.circle(extraction,(int(x), int(y)), 2,(0,255,0),3)
cv2.imshow("Image window", np.hstack([crop_img,extraction]))
cv2.waitKey(1)
yaw = 1500 + (x - width/2) * 1.5
print "center=" + str(width/2) + "point=" + str(x) + "yaw=" + str(yaw)
throttle = 1900
if (yaw > 1900):
yaw = 1900
elif (yaw < 1100):
yaw = 1100
msg = OverrideRCIn()
msg.channels[0] = yaw
msg.channels[1] = 0
msg.channels[2] = throttle
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
self.pub.publish(msg)
def callback(data):
frame = np.zeros((500, 500,3), np.uint8)
angle = data.angle_max
Vx = 250
Vy = 250
for r in data.ranges:
if r == float ('Inf'):
r = data.range_max
x = math.trunc( (r * 10)*math.cos(angle + (-90*3.1416/180)) )
y = math.trunc( (r * 10)*math.sin(angle + (-90*3.1416/180)) )
cv2.line(frame,(250, 250),(x+250,y+250),(255,0,0),1)
Vx+=x
Vy+=y
angle= angle - data.angle_increment
cv2.line(frame,(250, 250),(250+Vx,250+Vy),(0,0,255),3)
cv2.circle(frame, (250, 250), 2, (255, 255, 0))
ang = -(math.atan2(Vx,Vy)-3.1416)*180/3.1416
if ang > 180:
ang -= 360
cv2.putText(frame,str(ang)[:10], (50,400), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255))
cv2.imshow('frame',frame)
cv2.waitKey(1)
yaw = 1500 + ang * 40 / 6
throttle = 1900
msg = OverrideRCIn()
msg.channels[0] = yaw
msg.channels[1] = 0
msg.channels[2] = throttle
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
pub.publish(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 kill_command(self):
rc_msg = OverrideRCIn()
rc_msg.channels = [65535 for i in range(8)] # Stops override
self.pub_rc.publish(rc_msg)
def fly(self):
if self.buttons:
# Button 1 - enters failsafe mode (enables full control)
if self.buttons[ ctrl['failsafe'] ]:
self.reset_variables()
self.failsafe = True
print "Failsafe ON"
# Button 3 - arms the drone
if self.buttons[ ctrl['arm'] ]:
self.srv_arm(True)
print "Arming drone"
# Button 4 - disarms drone
if self.buttons[ ctrl['disarm'] ]:
self.srv_arm(False)
print "Disarming drone"
# Button 5 - initiate autonomy routine
if self.buttons[ ctrl['auto'] ]:
if self.drone.armed:
self.drone.mode = 2
else:
self.drone.mode = 1
self.failsafe = False
self.time_mode_started = millis()
print "Beginning finite state machine"
# Button 6 - end autonomy routine (RTL)
if self.buttons[ ctrl['manual'] ]:
self.drone.mode = 5
self.failsafe = False
self.time_mode_started = millis()
self.t1 = millis()
print "Returning to launch and landing"
# Debounces button, once pressed
time.sleep(0.1)
# Initiates finite state machines
if not self.failsafe:
# Arming
if self.drone.mode == 1:
if self.outdoors:
self.srv_mode(0, '5')
else:
self.srv_mode(0, '2')
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
if self.outdoors:
self.srv_mode(0, '5')
else:
self.srv_mode(0, '2')
self.drone.mode = 0
x = 1500 - self.axes[ ctrl['x'] ] * 300
y = 1500 - self.axes[ ctrl['y'] ] * 300
z = 1500 + self.axes[ ctrl['z'] ] * 500
yaw = 1500 - self.axes[ ctrl['yaw'] ] * 300
if self.just_armed:
z = 1000
if abs(self.axes[ ctrl['z'] ]) > 0.1:
self.just_armed = False
if z < 1200:
z = 1000
elif z < 1450:
z = 1300
elif z > 1650:
z = 1750
else:
z = 1500
if abs(x - 1500) < 50:
x = 1500
if abs(y - 1500) < 50:
y = 1500
(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, self.drone.cam_tilt, self.drone.cam_tilt, self.drone.cam_tilt]
self.pub_rc.publish(rc_msg)
self.pub_mode.publish(self.drone.mode)
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 __init__(self):
self.__rcChannels = OverrideRCIn().channels
loop_rate=rospy.Rate(30)
#Set the system ID to allow RC Override
#sml_setup.Set_System_ID(NODE_NAME)
#Publish to RC Override
rc_override=rospy.Publisher('mavros/rc/override',OverrideRCIn,queue_size=10)
#Publish for rqt_graph to show the positions of the quad
plot_pos_x=rospy.Publisher('/controller/plot_data_x',PlotData,queue_size=10)
plot_pos_y=rospy.Publisher('/controller/plot_data_y',PlotData,queue_size=10)
plot_pos_z=rospy.Publisher('/controller/plot_data_z',PlotData,queue_size=10)
#Set up service to request body data from Qualysis
body_info=sml_setup.Connect_To_Mocap(NODE_NAME)
data=OverrideRCIn()
command=[0,0,0,CONTROL_ARMING_MIN,0,0,0,0]
data.channels=command
rc_override.publish(data)
rospy.loginfo('['+NODE_NAME+']: Hovering at '+str(hovering_alt)+' meters...')
#Get initial body position
try:
body=body_info(body_id)
body_prev=body
except:
rospy.logerr('['+NODE_NAME+']: No connection to the mocap system')
Control_To_RC(rc_override)
sys.exit()
def GetRcMessage(self):
msg = OverrideRCIn()
msg.channels = self.__rcChannels
return msg
def GetRcMessage(self):
msg = OverrideRCIn()
msg.channels = self.__rcChannels
return 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)
