def set_mode(self, mode):
if not self.current_state.connected:
print "No FCU connection"
elif self.current_state.mode == mode:
print "Already in " + mode + " mode"
else:
# wait for service
rospy.wait_for_service("mavros/set_mode")
# service client
set_mode = rospy.ServiceProxy("mavros/set_mode", SetMode)
# set request object
req = SetModeRequest()
req.custom_mode = mode
# zero time
t0 = rospy.get_time()
# check response
while not rospy.is_shutdown() and (self.current_state.mode != req.custom_mode):
if rospy.get_time() - t0 > 2.0: # check every 5 seconds
try:
# request
set_mode.call(req)
except rospy.ServiceException, e:
print "Service did not process request: %s"%str(e)
t0 = rospy.get_time()
print "Mode: "+self.current_state.mode + " established"
评论列表
文章目录