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 handler_rc(data):
# global last_time
cmd = OverrideRCIn()
cmd.channels = data.channels
cmd_publisher.publish( cmd )
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 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 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()
#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()
#Get current time
current_time=rospy.Time.now()
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)
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[ 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 kill_command(self):
rc_msg = OverrideRCIn()
rc_msg.channels = [65535 for i in range(8)] # Stops override
self.pub_rc.publish(rc_msg)
