def new_joydata(self, joymsg):
global Button
global Axes
#Should run at 100hz. Use launch files roslaunch crazyflie_ros joy.launch
if self.prev_cmd == None:
self.prev_cmd = joymsg
print len(joymsg.axes)
#USB mode
if len(joymsg.axes) == 27:
rospy.logwarn("Experimental: Using USB mode. Axes/Buttons may be different")
Button = enum(L1=10,R1=11,Select=0,Start=3,L2=8,R2=9,Up=4,Right=5,Down=6,Left=7,Square=15,Cross=14,Triangle=12,Circle=13)
Axes = enum(SLL=0,SLU=1,SRL=2,SRU=3,Up=4+4,Right=5+4,Down=6+4,Left=7+4,L2=4+8,R2=4+9,L1=4+10,R1=4+11,Triangle=4+12,Circle=4+13,Cross=4+14,Square=4+15,AccL=16,AccF=17,AccU=18,GyroY=19)
self.USB = True
return
# bp = [i for i in range(len(joymsg.buttons)) if joymsg.buttons[i]]
# ap = [(i,round(joymsg.axes[i],2)) for i in range(len(joymsg.axes)) if abs(joymsg.axes[i])>0.51 ]
# if len(bp)>0:
# print "Buttons:", bp
# if len(ap)>0:
# print "Axes :", ap
if self.USB:
# USB buttons go from 1 to -1, Bluetooth go from 0 to -1, so normalise
joymsg.axes =np.array(joymsg.axes, dtype=np.float32)
joymsg.axes[8:19] -=1
joymsg.axes[8:19] /=2
# Strange bug: left button as axis doesnt work. So use button instead
joymsg.axes[11] = -joymsg.buttons[Button.Left]/2.
self.curr_cmd = joymsg
hover = False
x = 0
y = 0
z = 0
r = 0
r2 = 0
# Get stick positions [-1 1]
if self.curr_cmd.buttons[Button.L1]: # Deadman pressed (=allow flight)
x = self.joy_scale[0] * self.curr_cmd.axes[Axes.SLL] # Roll
y = self.joy_scale[1] * self.curr_cmd.axes[Axes.SLU] # Pitch
r = self.joy_scale[2] * self.curr_cmd.axes[Axes.SRL] # Yaw
z = self.joy_scale[3] * self.curr_cmd.axes[Axes.SRU] # Thrust
r2 = self.joy_scale[4] * (self.curr_cmd.axes[Axes.L2] - self.curr_cmd.axes[Axes.R2])
hover = self.curr_cmd.axes[Axes.L1]<-0.75
roll = x * self.max_angle
pitch = y * self.max_angle
yaw = 0
#TODO use dyn reconf to decide which to use
if r2!=0:
yaw = r2 * self.max_yawangle
else:
if self.yaw_joy:
# Deadzone
if r < -0.2 or r > 0.2:
if r < 0:
yaw = (r + 0.2) * self.max_yawangle * 1.25
else:
yaw = (r - 0.2) * self.max_yawangle * 1.25
if hover:
thrust = int(round(deadband(z,0.2)*32767 + 32767)) #Convert to uint16
else:
thrust = self.thurstToRaw(z)
trimmed_roll = roll + self.trim_roll
trimmed_pitch = pitch + self.trim_pitch
# Control trim manually
new_settings = {}
if joymsg.header.seq%10 == 0:
if self.curr_cmd.buttons[Button.Left]:
new_settings["trim_roll"] = max(self.trim_roll + self.curr_cmd.axes[Axes.Left], -20)
if self.curr_cmd.buttons[Button.Right]:
new_settings["trim_roll"] = min(self.trim_roll - self.curr_cmd.axes[Axes.Right], 20)
if self.curr_cmd.buttons[Button.Down]:
new_settings["trim_pitch"] = max(self.trim_pitch + self.curr_cmd.axes[Axes.Down], -20)
if self.curr_cmd.buttons[Button.Up]:
new_settings["trim_pitch"] = min(self.trim_pitch - self.curr_cmd.axes[Axes.Up], 20)
msg = CFJoyMSG()
msg.header.stamp = rospy.Time.now()
msg.roll = trimmed_roll
msg.pitch = trimmed_pitch
msg.yaw = yaw
msg.thrust = thrust
msg.hover = hover
#msg.calib = self.pressed(Button.Triangle)
# TODO: rotation is wrong! Cannot simply decompose x,y into r,p.
# TODO: roll, pitch should be dependent from the distance and the angle.
if self.PIDControl: # and hover:
try:
rtime = rospy.Time.now()
# Update goal position
if hover and joymsg.header.seq%10 == 0:
if self.PIDActive[0]:
if abs(x)>0.016:
new_settings["x"] = self.goal[0]-roll/70.
if abs(y)>0.016:
new_settings["y"] = self.goal[1]-pitch/70.
if self.PIDActive[1]:
if abs(z)>0.016:
new_settings["z"] = self.goal[2]+z/5
if self.PIDActive[2]:
if abs(yaw)>0:
new_settings["rz"] = self.goal[3]-yaw/self.max_yawangle*20
if new_settings.has_key("x") or new_settings.has_key("y") or new_settings.has_key("z") or new_settings.has_key("rz"):
os.system("beep -f 6000 -l 15&")
# Get error from current position to goal if possible
[px,py,alt,rz] = self.getErrorToGoal()
# Starting control mode
if hover and not self.prev_msg.hover:
# Starting Goal should be from current position
if self.PIDSetCurrentAuto:
new_settings["SetCurrent"] = True
# Reset PID controllers
for p in range(4):
self.PID[p].reset(rtime)
# Set yaw difference as I part
#self.PID[3].error_sum = 0
if hover:
msg.hover = False #TODO should be an option
pidmsg = PidMSG()
pidmsg.header.stamp = rtime
# Get PID control
#XY control depends on the asin of the distance
#cr, crp, cri, crd = self.PID[0].get_command(-py,rtime,f=lambda d: degrees(asin(d))/90.*self.max_angle)
#cp, cpp, cpi, cpd = self.PID[1].get_command(px,rtime,f=lambda d: degrees(asin(d))/90.*self.max_angle)
#cr, crp, cri, crd = self.PID[0].get_command(-py,rtime)
#cp, cpp, cpi, cpd = self.PID[1].get_command(px,rtime)
# Distance to angle (X direction = pitch, Y direction = roll)
cr, crp, cri, crd = self.PID[0].get_command(-py,rtime,f=self.distFunc )
cp, cpp, cpi, cpd = self.PID[1].get_command(px,rtime,f=self.distFunc )
# THRUST
ct, ctp, cti, ctd = self.PID[2].get_command(alt,rtime)
# YAW VEL
cy, cyp, cyi, cyd = self.PID[3].get_command(-rz,rtime)#,d=rz,i=copysign(1,-rz)) #TODO: normalise rotation
# ROll/Pitch
if self.PIDActive[0]:
msg.roll = cr + self.trim_roll
msg.pitch = cp + self.trim_pitch
pidmsg.ypid = [cr, crp, cri, -crd, -py]
pidmsg.xpid = [cp, cpp, cpi, -cpd, px]
# THRUST
if self.PIDActive[1]:
# Add base thrust
ct += self.thrust[2]
# Add roll compensation
# TODO: done on the flie itself now @ 250hz
# Bound
ct = max(self.thrust[0],ct)
ct = min(self.thrust[1],ct)
# ct in thrust percent
msg.thrust = percentageToThrust(ct)
pidmsg.zpid= [ct, ctp, cti, ctd, alt]
# YAW
if self.PIDActive[2]:
msg.yaw = cy
pidmsg.rzpid= [cy, cyp, cyi, cyd, -rz]
self.pub_PID.publish(pidmsg)
if self.pressed(Button.Cross):
new_settings["SetCurrent"] = True
q = tf.transformations.quaternion_from_euler(radians(msg.roll-self.trim_roll), radians(msg.pitch - self.trim_pitch), radians(-msg.yaw))
self.pub_tf.sendTransform((0,0,0), q, rospy.Time.now(), "/cmd", "/cf_gt2d")
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
if time.time() - self.warntime>2:
rospy.logwarn('Could not look up cf_gt2d -> goal')
self.warntime = time.time()
global tid
if self.pressed(Button.Circle):
tid = (tid +1)%len(trajectory)
new_settings["x"], new_settings["y"], new_settings["z"], new_settings["rz"] = trajectory[tid]
# previous
if self.pressed(Button.Triangle):
tid = (tid -1)%len(trajectory)
new_settings["x"], new_settings["y"], new_settings["z"], new_settings["rz"] = trajectory[tid]
# Set trim to current input
if self.released(Button.R1):
new_settings["trim_roll"] = min(20, max(msg.roll, -20))
new_settings["trim_pitch"] = min(20, max(msg.pitch, -20))
rospy.loginfo("Trim updated Roll/Pitch: %r/%r", round(new_settings["trim_roll"],2), round(new_settings["trim_pitch"],2))
# Reset Trim
if self.released(Button.Square):
new_settings["trim_roll"] = 0.0
new_settings["trim_pitch"] = 0.0
rospy.loginfo("Pitch reset to 0/0")
if new_settings != {} and self.dynserver is not None:
if self.USB:
new_settings = numpy2python(new_settings)
self.dynserver.update_configuration(new_settings)
# Cache prev joy command
self.pub_cfJoy.publish(msg)
self.prev_cmd = self.curr_cmd
self.prev_msg = msg
self.prev_msg.hover = hover
def new_joydata(self, joymsg):
global Button
global Axes
#Should run at 100hz. Use launch files roslaunch crazyflie_ros joy.launch
if self.prev_cmd == None:
self.prev_cmd = joymsg
print len(joymsg.axes)
#USB mode
if len(joymsg.axes) == 27:
Button = enum(L1=10,R1=11,Select=0,Start=3,L2=8,R2=9,Up=4,Right=5,Down=6,Left=7,Square=15,Cross=14,Triangle=12,Circle=13)
Axes = enum(SLL=0,SLU=1,SRL=2,SRU=3,Up=4,Right=5,Down=6,Left=7,L2=4+8,R2=4+9,L1=4+10,R1=4+11,Triangle=4+12,Circle=4+13,Cross=4+14,Square=4+15,AccL=16,AccF=17,AccU=18,GyroY=19)
return
self.curr_cmd = joymsg
hover = False
# print "AXES"
# for i,x in enumerate(joymsg.axes):
# print " ",i,":",x
# print "BUTTONS
# for i,x in enumerate(joymsg.buttons):
# print " ",i,":",x
x = 0
y = 0
z = 0
r = 0
r2 = 0
# Get stick positions [-1 1]
if self.curr_cmd.buttons[Button.L1]:
x = self.joy_scale[0] * self.curr_cmd.axes[Axes.SLL] # Roll
y = self.joy_scale[1] * self.curr_cmd.axes[Axes.SLU] # Pitch
r = self.joy_scale[2] * self.curr_cmd.axes[Axes.SRL] # Yaw
z = self.joy_scale[3] * self.curr_cmd.axes[Axes.SRU] # Thrust
r2 = self.joy_scale[4] * (self.curr_cmd.axes[Axes.L2] - self.curr_cmd.axes[Axes.R2])
hover = self.curr_cmd.axes[Axes.L1]<-0.75
roll = x * self.max_angle
pitch = y * self.max_angle
yaw = 0
#TODO use dyn reconf to decide which to use
if r2!=0:
yaw = r2 * self.max_yawangle
else:
if self.yaw_joy:
# Deadzone
if r < -0.2 or r > 0.2:
if r < 0:
yaw = (r + 0.2) * self.max_yawangle * 1.25
else:
yaw = (r - 0.2) * self.max_yawangle * 1.25
if hover:
thrust = int(round(deadband(z,0.2)*32767 + 32767)) #Convert to uint16
else:
thrust = self.thurstToRaw(z)
trimmed_roll = roll + self.trim_roll
trimmed_pitch = pitch + self.trim_pitch
# Control trim manually
new_settings = {}
if self.curr_cmd.buttons[Button.Left]:
new_settings["trim_roll"] = max(self.trim_roll + self.curr_cmd.axes[Axes.Left]/10.0, -10)
if self.curr_cmd.buttons[Button.Right]:
new_settings["trim_roll"] = min(self.trim_roll - self.curr_cmd.axes[Axes.Right]/10.0, 10)
if self.curr_cmd.buttons[Button.Down]:
new_settings["trim_pitch"] = max(self.trim_pitch + self.curr_cmd.axes[Axes.Down]/10.0, -10)
if self.curr_cmd.buttons[Button.Up]:
new_settings["trim_pitch"] = min(self.trim_pitch - self.curr_cmd.axes[Axes.Up]/10.0, 10)
# Set trim to current input
if self.released(Button.R1):
new_settings["trim_roll"] = min(10, max(trimmed_roll, -10))
new_settings["trim_pitch"] = min(10, max(trimmed_pitch, -10))
rospy.loginfo("Trim updated Roll/Pitch: %r/%r", round(new_settings["trim_roll"],2), round(new_settings["trim_roll"],2))
# Reset Trim
if self.released(Button.Square):
new_settings["trim_roll"] = 0
new_settings["trim_pitch"] = 0
rospy.loginfo("Pitch reset to 0/0")
if new_settings != {} and self.dynserver!=None:
self.dynserver.update_configuration(new_settings)
# TODO: new firmware doesnt need hover_change, it just uses thrust directly
#(trimmed_roll,trimmed_pitch,yaw,thrust,hover,hover_set, z)
msg = CFJoyMSG()
msg.header.stamp = rospy.Time.now()
msg.roll = trimmed_roll
msg.pitch = trimmed_pitch
msg.yaw = yaw
msg.thrust = thrust
msg.hover = hover
#msg.calib = self.pressed(Button.Triangle)
self.pub_cfJoy.publish(msg)
# Cache prev joy command
self.prev_cmd = self.curr_cmd