utils.py

Go to the documentation of this file.

# -*- coding: utf-8 -*-
# vim:set ts=4 sw=4 et:
#
# Copyright 2014 Vladimir Ermakov.
#
# This file is part of the mavros package and subject to the license terms
# in the top-level LICENSE file of the mavros repository.
# https://github.com/mavlink/mavros/tree/master/LICENSE.md
 
from __future__ import print_function
 
import os
import sys
 
import rospy
import mavros
import threading
 
 
def print_if(cond, *args, **kvargs):
    if cond:
        print(*args, **kvargs)
 
 
def fault(*args, **kvargs):
    kvargs['file'] = sys.stderr
    print(*args, **kvargs)
    sys.exit(1)
 
 
def wait_fcu_connection(timeout=None):
    """
    Wait until establishing FCU connection
    """
    from mavros_msgs.msg import State
    try:
        msg = rospy.wait_for_message(mavros.get_topic('state'), State, timeout)
        if msg.connected:
            return True
    except rospy.ROSException as e:
        return False
 
    connected = threading.Event()
    def handler(msg):
        if msg.connected:
            connected.set()
 
    sub = rospy.Subscriber(
        mavros.get_topic('state'),
        State,
        handler
    )
 
    return connected.wait(timeout)