armCommandSrv = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool)
setModeSrv = rospy.ServiceProxy("/mavros/set_mode", SetMode) #http://wiki.ros.org/mavros/CustomModes
#msgs sent at 60hz
rate = rospy.Rate(10)
#creating pose msg
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = .5
#StateMachine msg that will switch Lakitu to 'hover' state
state = StateMachine()
state.preflight = False
state.takeoff = False
state.flight = False
state.hover = True
state.land = False
state.emergency = False
# last_request = rospy.Time.now()
#main loop of program
while not rospy.is_shutdown():
if(flight_state is None):
# print('what the f**k')
continue
if(current_state is None):
continue
setModeSrv = rospy.ServiceProxy(
"/mavros/set_mode", SetMode) #http://wiki.ros.org/mavros/CustomModes
#msgs sent at 60hz
rate = rospy.Rate(60)
#creating pose msg
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 0
#StateMachine msg that will switch Lakitu to 'takeoff' state
state = StateMachine()
state.preflight = False
state.takeoff = True
state.flight = False
state.hover = False
state.land = False
state.emergency = False
last_request = rospy.Time.now()
#main loop of program
while not rospy.is_shutdown():
#publishes 'empty' local pose to allow offboard mode
# local_pos_pub.publish(pose)
if (preflight_state is None):
continue