utils.py
Go to the documentation of this file.
00001 # -*- coding: utf-8 -*-
00002 # vim:set ts=4 sw=4 et:
00003 #
00004 # Copyright 2014 Vladimir Ermakov.
00005 #
00006 # This file is part of the mavros package and subject to the license terms
00007 # in the top-level LICENSE file of the mavros repository.
00008 # https://github.com/mavlink/mavros/tree/master/LICENSE.md
00009 
00010 from __future__ import print_function
00011 
00012 import os
00013 import sys
00014 
00015 import rospy
00016 import mavros
00017 import threading
00018 
00019 
00020 def print_if(cond, *args, **kvargs):
00021     if cond:
00022         print(*args, **kvargs)
00023 
00024 
00025 def fault(*args, **kvargs):
00026     kvargs['file'] = sys.stderr
00027     print(*args, **kvargs)
00028     sys.exit(1)
00029 
00030 
00031 def wait_fcu_connection(timeout=None):
00032     """
00033     Wait until establishing FCU connection
00034     """
00035     from mavros_msgs.msg import State
00036     try:
00037         msg = rospy.wait_for_message(mavros.get_topic('state'), State, timeout)
00038         if msg.connected:
00039             return True
00040     except rospy.ROSException as e:
00041         return False
00042 
00043     connected = threading.Event()
00044     def handler(msg):
00045         if msg.connected:
00046             connected.set()
00047 
00048     sub = rospy.Subscriber(
00049         mavros.get_topic('state'),
00050         State,
00051         handler
00052     )
00053 
00054     return connected.wait(timeout)


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19