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()