def init():
global tko
global land
global linX
global linY
global Alt
global Hdg
rospy.init_node('mambo_node',anonymous=True)
mamboAdd = rospy.get_param('~Mambo_Address',str("e0:14:7d:11:3d:c7"))
wifi = rospy.get_param('~mambo_wifi',False)
retries = rospy.get_param('~mambo_retries',3)
rospy.loginfo("\n" + rospy.get_name() + "\nParameters:\n" + mamboAdd + "\n" + str(wifi) + "\n" + str(retries) +"\n")
s_cmd_vel = rospy.Subscriber('cmd_vel',Twist,cb_cmd_vel)
s_take_off = rospy.Subscriber('take_off',Empty,cb_take_off)
s_land = rospy.Subscriber('land',Empty,cb_land)
mambo = Mambo(mamboAdd,use_wifi=wifi)
success = mambo.connect(retries)
if(success):
mambo.smart_sleep(2)
mambo.ask_for_state_update()
mambo.smart_sleep(2)
mambo.flat_trim()
rate = rospy.Rate(60)
while not rospy.is_shutdown():
# mambo.ask_for_state_update()
if tko == True:
mambo.safe_takeoff(7)
tko = False
if land == True:
mambo.safe_land(4)
land = False
mambo.fly_direct(roll = (linY * 100), pitch = (linX*100),yaw = (Hdg *100), vertical_movement = (Alt*100), duration=0.001)
rate.sleep()
def init():
global tko
global land
global cannon
global auto_tko
global linX
global linY
global Alt
global Hdg
global p_mode
global need_to_change_mode
global need_to_toggle_mode
rospy.init_node('mambo_node', anonymous=True)
mamboAdd = rospy.get_param('~bt', str("e0:14:60:5c:3d:c7"))
wifi = rospy.get_param('~mambo_wifi', False)
retries = rospy.get_param('~mambo_retries', 3)
rospy.loginfo("\n" + rospy.get_name() + "\nParameters:\n" + mamboAdd +
"\n" + str(wifi) + "\n" + str(retries) + "\n")
s_cmd_vel = rospy.Subscriber('cmd_vel', Twist, cb_cmd_vel)
s_take_off = rospy.Subscriber('take_off', Empty, cb_take_off)
s_land = rospy.Subscriber('land', Empty, cb_land)
s_cannon = rospy.Subscriber('cannon', Empty, cb_shoot_cannon)
s_auto_tko = rospy.Subscriber('auto_tko', Empty, cb_auto_take_off)
s_piloting_mode = rospy.Subscriber('piloting_mode', UInt8, cb_pilotmode)
s_toggle_mode = rospy.Subscriber('toggle_mode', Empty, cb_toggle_mode)
p_ready = rospy.Publisher('ready', String, queue_size=30)
mambo = Mambo(mamboAdd, use_wifi=wifi)
success = mambo.connect(retries)
if (success):
mambo.smart_sleep(2)
mambo.ask_for_state_update()
mambo.smart_sleep(2)
mambo.flat_trim()
rate = rospy.Rate(100)
while not rospy.is_shutdown():
if tko == True:
mambo.safe_takeoff(2)
p_ready.publish("ok")
tko = False
if land == True:
mambo.safe_land(2)
land = False
if cannon == True:
mambo.fire_gun()
cannon = False
if auto_tko == True:
mambo.turn_on_auto_takeoff()
auto_tko = False
if need_to_change_mode == True:
if pilotmode(mambo, p_mode):
print("Changed mode successfully")
else:
print("Failed changing mode")
need_to_change_mode = False
if need_to_toggle_mode == True:
if togglemode(mambo):
print("Activating preffered mode...")
else:
print("Failed activating preferred mode")
need_to_toggle_mode = False
r_y = round(linY, 2)
p_x = round(linX, 2)
v_z = round(Alt, 2)
y_z = round(Hdg, 2)
r_y = sat(r_y, 0.98)
p_x = sat(p_x, 0.98)
v_z = sat(v_z, 0.98)
y_z = sat(y_z, 0.98)
mambo.fly_direct(roll=(-r_y * 100),
pitch=(p_x * 100),
yaw=(-y_z * 100),
vertical_movement=(v_z * 100),
duration=0.01)
# linY = 0
# linX = 0
# Hdg = 0
# Alt = 0
rate.sleep()
