from sensor_msgs.msg import Range
from mavros_msgs.msg import State, PositionTarget
from mavros_msgs.srv import SetMode, CommandBool
from std_msgs.msg import Bool
from std_srvs.srv import Trigger, TriggerResponse
from geometry_msgs.msg import PoseStamped
# Add parent dir to PATH to import messaging_lib and config_lib
current_dir = (os.path.dirname(os.path.realpath(__file__)))
lib_dir = os.path.realpath(os.path.join(current_dir, '../lib'))
sys.path.insert(0, lib_dir)
from config import ConfigManager
config = ConfigManager()
config.load_config_and_spec("config/client.ini")
watchdog_is_enabled = config.failsafe_enabled
log_state = config.failsafe_log_state
vision_pose_delay_after_arm = config.failsafe_vision_pose_delay_after_arm
visual_pose_timeout = config.failsafe_vision_pose_timeout
pos_delta_max = config.failsafe_position_delta_max
watchdog_action = config.failsafe_action
timeout_to_disarm = config.failsafe_disarm_timeout
emergency_land_thrust = config.emergency_land_thrust
emergency_land_decrease_thrust_after = config.emergency_land_decrease_thrust_after
logging.basicConfig( # TODO all prints as logs
level=logging.DEBUG, # INFO
stream=sys.stdout,
format=
config_path = 'animation_config/config'
spec_path = os.path.join(config_path, 'spec')
if not os.path.exists(spec_path):
try:
os.makedirs(spec_path)
except OSError:
print("Creation of the directory {} failed".format(spec_path))
else:
print("Successfully created the directory {}".format(spec_path))
configspec_path = os.path.realpath(
os.path.join(root_dir, "drone/config/spec/configspec_client.ini"))
shutil.copy(configspec_path, spec_path)
config = ConfigManager()
config.load_config_and_spec(os.path.join(config_path, 'client.ini'))
assert config.config_name == "client"
import animation
assets_dir = os.path.realpath(os.path.join(root_dir, 'drone/tests/assets'))
def test_animation_1_2():
a = animation.Animation(os.path.join(assets_dir, 'animation_1.csv'),
config)
assert a.id == 'basic'
assert a.state == "OK"
assert approx(a.original_frames[0].get_pos()) == [0, 0, 0]
assert a.original_frames[0].get_color() == [204, 2, 0]
class Client(object):
def __init__(self, config_path="config/client.ini"):
self.selector = selectors.DefaultSelector()
self.client_socket = None
self.server_connection = messaging.ConnectionManager()
self.connected = False
self.client_id = None
# Init configs
self.config = ConfigManager()
self.config_path = config_path
global active_client
active_client = self
def load_config(self):
self.config.load_config_and_spec(self.config_path)
config_id = self.config.id.lower()
if config_id == '/default':
self.client_id = 'copter' + str(random.randrange(9999)).zfill(4)
self.config.set('', 'id', self.client_id,
write=True) # set and write
elif config_id == '/hostname':
self.client_id = socket.gethostname()
elif config_id == '/ip':
self.client_id = messaging.get_ip_address()
else:
self.client_id = config_id
logger.info("Config loaded")
@staticmethod
def get_ntp_time(ntp_host, ntp_port):
NTP_PACKET_FORMAT = "!12I"
NTP_DELTA = 2208988800L # 1970-01-01 00:00:00
NTP_QUERY = '\x1b' + 47 * '\0'
with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as s:
s.sendto(bytes(NTP_QUERY), (ntp_host, ntp_port))
msg, address = s.recvfrom(1024)
unpacked = struct.unpack(NTP_PACKET_FORMAT,
msg[0:struct.calcsize(NTP_PACKET_FORMAT)])
return unpacked[10] + float(unpacked[11]) / 2**32 - NTP_DELTA
def time_now(self):
if self.config.ntp_use:
timenow = self.get_ntp_time(self.config.ntp_host,
self.config.ntp_port)
else:
timenow = time.time()
return timenow
def start(self):
self.load_config()
logger.info("Starting client")
messaging.NotifierSock().init(self.selector)
try:
while True:
self._reconnect()
self._process_connections()
except (KeyboardInterrupt, ):
logger.critical("Caught interrupt, exiting!")
self.selector.close()
def _reconnect(self,
timeout=2.0,
attempt_limit=3
): # TODO reconnecting broadcast listener in another thread
logger.info("Trying to connect to {}:{} ...".format(
self.config.server_host, self.config.server_port))
attempt_count = 0
while not self.connected:
logger.info(
"Waiting for connection, attempt {}".format(attempt_count))
try:
self.client_socket = socket.socket()
self.client_socket.settimeout(timeout)
messaging.set_keepalive(self.client_socket)
self.client_socket.setsockopt(socket.IPPROTO_TCP,
socket.TCP_NODELAY, 1)
self.client_socket.connect(
(self.config.server_host, self.config.server_port))
except socket.error as error:
if isinstance(error, OSError):
if error.errno == errno.EINTR:
logger.critical("Shutting down on keyboard interrupt")
raise KeyboardInterrupt
logger.warning("Can not connect due error: {}".format(error))
attempt_count += 1
time.sleep(timeout)
else:
logger.info("Connection to server successful!")
self._connect()
break
if attempt_count >= attempt_limit:
logger.info("Too many attempts. Trying to get new server IP")
self.broadcast_bind(timeout * 2, attempt_limit)
attempt_count = 0
def _connect(self):
self.connected = True
self.client_socket.setblocking(False)
self.selector.register(self.client_socket,
selectors.EVENT_READ,
data=self.server_connection)
self.server_connection.connect(
self.selector, self.client_socket,
(self.config.server_host, self.config.server_port))
def broadcast_bind(self, timeout=2.0, attempt_limit=3):
broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
broadcast_client.settimeout(timeout)
try:
broadcast_client.bind(("", self.config.broadcast_port))
except socket.error as error:
logger.error(
"Error during broadcast listening binding: {}".format(error))
return
attempt_count = 0
try:
while attempt_count <= attempt_limit:
try:
data, addr = broadcast_client.recvfrom(
self.config.server_buffer_size)
except socket.error as error:
logger.warning(
"Could not receive broadcast due error: {}".format(
error))
attempt_count += 1
else:
message = messaging.MessageManager()
message.income_raw = data
message.process_message()
if message.content and message.jsonheader[
"action"] == "server_ip":
logger.info(
"Received broadcast message {} from {}".format(
message.content, addr))
kwargs = message.content["kwargs"]
self.config.set("SERVER", "port", int(kwargs["port"]))
self.config.set("SERVER", "host", kwargs["host"])
self.config.write()
logger.info("Binding to new IP: {}:{}".format(
self.config.server_host, self.config.server_port))
self.on_broadcast_bind()
break
finally:
broadcast_client.close()
def on_broadcast_bind(self): # TODO move ALL binding code here
pass
def _process_connections(self):
while True:
events = self.selector.select(timeout=1)
for key, mask in events:
connection = key.data
if connection is not None:
try:
connection.process_events(mask)
except Exception as error:
logger.error(
"Exception {} occurred for {}! Resetting connection!"
.format(error, connection.addr))
self.server_connection._close()
self.connected = False
if isinstance(error, OSError):
if error.errno == errno.EINTR:
raise KeyboardInterrupt
try:
mapping_fds = self.selector.get_map().keys(
) # file descriptors
notifier_fd = messaging.NotifierSock().get_sock().fileno()
except (KeyError, RuntimeError) as e:
logger.error(
"Exception {} occurred when getting connections map!".
format(e))
logger.error(
"Connections changed during getting connections map, passing"
)
else:
notify_only = len(
mapping_fds) == 1 and notifier_fd in mapping_fds
if notify_only or not mapping_fds:
logger.warning("No active connections left!")
return
class Server(messaging.Singleton):
def __init__(self, config_path="../config/server.ini", server_id=None):
self.id = server_id if server_id else str(random.randint(0, 9999)).zfill(4)
self.time_started = 0
# Init socket
self.sel = selectors.DefaultSelector()
self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
messaging.set_keepalive(self.server_socket)
self.server_socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
self.host = socket.gethostname()
self.ip = messaging.get_ip_address()
# Init configs
self.config = ConfigManager()
self.config_path = config_path
# Init threads
self.autoconnect_thread = threading.Thread(target=self._client_processor, daemon=True,
name='Client processor')
self.client_processor_thread_running = threading.Event() # Can be used for manual thread killing
self.broadcast_thread = threading.Thread(target=self._ip_broadcast, daemon=True,
name='IP broadcast sender')
self.broadcast_thread_running = threading.Event() # TODO replace by interrupt
self.broadcast_thread_interrupt = threading.Event()
self.listener_thread = threading.Thread(target=self._broadcast_listen, daemon=True,
name='IP broadcast listener')
self.listener_thread_running = threading.Event()
def load_config(self):
self.config.load_config_and_spec(self.config_path)
def start(self):
# load config on startup
self.load_config()
self.time_started = time.time()
logging.info("Starting server with id: {} on {}:{} ({})!".format(self.id, self.ip, self.config.server_port,
socket.gethostname()))
logging.info("Binding server socket!")
self.server_socket.bind((self.ip, self.config.server_port))
logging.info("Starting client processor thread!")
self.client_processor_thread_running.set()
self.autoconnect_thread.start()
if self.config.broadcast_send:
logging.info("Starting broadcast sender thread!")
self.broadcast_thread_running.set()
self.broadcast_thread.start()
if self.config.broadcast_listen:
logging.info("Starting broadcast listener thread!")
self.listener_thread_running.set()
self.listener_thread.start()
def stop(self):
logging.info("Stopping server")
self.client_processor_thread_running.clear()
self.broadcast_thread_interrupt.set()
self.broadcast_thread_running.clear()
self.listener_thread_running.clear()
messaging.NotifierSock().notify()
self.server_socket.close()
self.sel.close()
messaging.NotifierSock().close()
logging.info("Server stopped")
def terminate(self, reason="Terminated"):
self.stop()
logging.critical(reason)
@staticmethod
def get_ntp_time(ntp_host, ntp_port):
NTP_DELTA = 2208988800 # 1970-01-01 00:00:00
NTP_QUERY = b'\x1b' + bytes(47)
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as ntp_socket:
ntp_socket.sendto(NTP_QUERY, (ntp_host, ntp_port))
msg, _ = ntp_socket.recvfrom(1024)
return int.from_bytes(msg[-8:], 'big') / 2 ** 32 - NTP_DELTA
def time_now(self):
if self.config.ntp_use:
return self.get_ntp_time(self.config.ntp_host, self.config.ntp_port)
return time.time()
# noinspection PyArgumentList
def _client_processor(self):
logging.info("Client processor (selector) thread started!")
messaging.NotifierSock().init(self.sel)
self.server_socket.listen()
self.server_socket.setblocking(False)
self.sel.register(self.server_socket, selectors.EVENT_READ, data=None)
while self.client_processor_thread_running.is_set():
events = self.sel.select(timeout=1)
for key, mask in events:
client = key.data
if client is None:
self._connect_client(key.fileobj)
elif isinstance(client, messaging.ConnectionManager):
try:
client.process_events(mask)
except Exception as error:
logging.error("Exception {} occurred for {}! Resetting connection!".format(error, client.addr))
traceback.print_exc()
client.close(True)
else: # Notifier
client.process_events(mask)
logging.info("Client autoconnect thread stopped!")
def _connect_client(self, sock):
try:
conn, addr = sock.accept()
except OSError:
logging.error("Error while connecting socket!")
return
logging.info("Got connection from: {}".format(str(addr)))
conn.setblocking(False)
if not any([client_addr == addr[0] for client_addr in Client.clients.keys()]):
client = Client(addr[0])
client.buffer_size = self.config.server_buffer_size
logging.info("New client")
else:
client = Client.clients[addr[0]]
client.close(True) # to ensure in unregistering
logging.info("Reconnected client")
self.sel.register(conn, selectors.EVENT_READ, data=client)
client.connect(self.sel, conn, addr)
def _ip_broadcast(self):
logging.info("Broadcast sender thread started!")
msg = messaging.MessageManager.create_action_message(
"server_ip", kwargs={"host": self.ip, "port": str(self.config.server_port), "id": self.id,
"start_time": str(self.time_started)})
logging.debug("Formed broadcast message to {}:{}: {}".format(self.config.broadcast_send_ip, self.config.broadcast_port, msg))
broadcast_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
broadcast_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
broadcast_sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
try:
while self.broadcast_thread_running.is_set():
self.broadcast_thread_interrupt.wait(timeout=self.config.broadcast_delay)
try:
broadcast_sock.sendto(msg, (self.config.broadcast_send_ip, self.config.broadcast_port))
except OSError as e:
logging.error(f"Cannot send broadcast due error {e}")
else:
logging.debug("Broadcast sent")
except Exception as e:
logging.error(f"Unexpected error {e}!")
raise
def _broadcast_listen(self):
logging.info("Broadcast listener thread started!")
broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
# broadcast_client.settimeout(1)
try:
broadcast_client.bind(("", self.config.broadcast_port))
except OSError:
self.terminate("Another server is running on this computer, shutting down!")
return
try:
while self.listener_thread_running.is_set():
try:
data, addr = broadcast_client.recvfrom(1024) # TODO nonblock
except OSError:
logging.error(f"Cannot receive broadcast due error {e}")
continue
message = messaging.MessageManager()
message.income_raw = data
message.process_message()
content = message.content
right_command = (content and message.jsonheader["action"] == "server_ip")
if right_command:
different_id = content["kwargs"]["id"] != str(self.id)
self_younger = float(content["kwargs"]["start_time"]) <= self.time_started
if different_id and self_younger:
# younger server should shut down
self.terminate("Another server detected over the network, shutting down!")
else:
logging.warning("Got wrong broadcast message from {}".format(addr))
except Exception as e:
logging.error(f"Unexpected error {e}!")
raise
finally:
broadcast_client.close()
logging.info("Broadcast listener thread stopped, socked closed!")
def send_starttime(self, copter, start_time):
copter.send_message("start", kwargs={"time": str(start_time)})
class Client(object):
"""
Client base class provides config loading, communication with server (including automatic reconnection, broadcast listening and binding). You can inherit this class in order to extend functionality for practical applications.
Attributes:
server_connection (ConnectionManager) - connection to the server.
connected (bool) - whether the client is connected to the server.
client_id (string) - ID of the client.
config (ConfigManager) - contains loaded client configuration.
config_path (string) - path to configuration file. There also should be config specification file at 'config_path\config\configspec_client.ini'.
"""
def __init__(self,
config_path=os.path.join(current_dir, os.pardir, "config",
"client.ini")):
"""
Initializtion
```python
client = Client(config_path)
```
Args:
config_path (string, optional): Path to the file with configuration. There also should be config specification file at `<config_path>\config\configspec_client.ini`. Defaults to `<current_dir>\os.pardir\config\client.ini`.
"""
self.selector = selectors.DefaultSelector()
self.client_socket = None
self.server_connection = messaging.ConnectionManager()
self.connected = False
self.client_id = None
# Init configs
self.config = ConfigManager()
self.config_path = config_path
global active_client
active_client = self
def load_config(self):
"""
Loads or reloads config from file specified in 'config_path' attribute.
"""
self.config.load_config_and_spec(self.config_path)
config_id = self.config.id.lower()
if config_id == '/default':
self.client_id = 'copter' + str(random.randrange(9999)).zfill(4)
self.config.set('', 'id', self.client_id,
write=True) # set and write
elif config_id == '/hostname':
self.client_id = socket.gethostname()
elif config_id == '/ip':
self.client_id = messaging.get_ip_address()
else:
self.client_id = config_id
logger.info("Config loaded")
@staticmethod
def get_ntp_time(ntp_host, ntp_port):
"""Gets and returns time from specified host and port of NTP server.
Args:
ntp_host (string): hostname or address of the NTP server.
ntp_port (int): port of the NTP server.
Returns:
int: Current time recieved from the NTP server
"""
NTP_PACKET_FORMAT = "!12I"
NTP_DELTA = 2208988800 # 1970-01-01 00:00:00
NTP_QUERY = '\x1b' + 47 * '\0'
with closing(socket.socket(socket.AF_INET, socket.SOCK_DGRAM)) as s:
s.sendto(bytes(NTP_QUERY), (ntp_host, ntp_port))
msg, address = s.recvfrom(1024)
unpacked = struct.unpack(NTP_PACKET_FORMAT,
msg[0:struct.calcsize(NTP_PACKET_FORMAT)])
return unpacked[10] + float(unpacked[11]) / 2**32 - NTP_DELTA
def time_now(self):
"""gets and returns system time or NTP time depending on the config.
Returns:
int: Current time.
"""
if self.config.ntp_use:
timenow = self.get_ntp_time(self.config.ntp_host,
self.config.ntp_port)
else:
timenow = time.time()
return timenow
def start(self):
"""
Reloads config and starts infinite loop of connecting to the server and processing said connection. Calling of this method will indefinitely halt execution of any subsequent code.
"""
self.load_config()
logger.info("Starting client")
messaging.NotifierSock().init(self.selector)
try:
while True:
self._reconnect()
self._process_connections()
except (KeyboardInterrupt, ):
logger.critical("Caught interrupt, exiting!")
self.selector.close()
def _reconnect(self,
timeout=2.0,
attempt_limit=3
): # TODO reconnecting broadcast listener in another thread
logger.info("Trying to connect to {}:{} ...".format(
self.config.server_host, self.config.server_port))
attempt_count = 0
while not self.connected:
logger.info(
"Waiting for connection, attempt {}".format(attempt_count))
try:
self.client_socket = socket.socket()
self.client_socket.settimeout(timeout)
messaging.set_keepalive(self.client_socket)
self.client_socket.setsockopt(socket.IPPROTO_TCP,
socket.TCP_NODELAY, 1)
self.client_socket.connect(
(self.config.server_host, self.config.server_port))
except socket.error as error:
if isinstance(error, OSError):
if error.errno == errno.EINTR:
logger.critical("Shutting down on keyboard interrupt")
raise KeyboardInterrupt
logger.warning("Can not connect due error: {}".format(error))
attempt_count += 1
time.sleep(timeout)
else:
logger.info("Connection to server successful!")
self._connect()
break
if attempt_count >= attempt_limit:
logger.info("Too many attempts. Trying to get new server IP")
self.broadcast_bind(timeout * 2, attempt_limit)
attempt_count = 0
def _connect(self):
self.connected = True
self.client_socket.setblocking(False)
self.selector.register(self.client_socket,
selectors.EVENT_READ,
data=self.server_connection)
self.server_connection.connect(
self.selector, self.client_socket,
(self.config.server_host, self.config.server_port))
def broadcast_bind(self, timeout=2.0, attempt_limit=3):
broadcast_client = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
broadcast_client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
broadcast_client.settimeout(timeout)
try:
broadcast_client.bind(("", self.config.broadcast_port))
except socket.error as error:
logger.error(
"Error during broadcast listening binding: {}".format(error))
return
attempt_count = 0
try:
while attempt_count <= attempt_limit:
try:
data, addr = broadcast_client.recvfrom(
self.config.server_buffer_size)
except socket.error as error:
logger.warning(
"Could not receive broadcast due error: {}".format(
error))
attempt_count += 1
else:
message = messaging.MessageManager()
message.income_raw = data
message.process_message()
if message.content and message.jsonheader[
"action"] == "server_ip":
logger.info(
"Received broadcast message {} from {}".format(
message.content, addr))
kwargs = message.content["kwargs"]
self.config.set("SERVER", "port", int(kwargs["port"]))
self.config.set("SERVER", "host", kwargs["host"])
self.config.write()
logger.info("Binding to new IP: {}:{}".format(
self.config.server_host, self.config.server_port))
self.on_broadcast_bind()
break
finally:
broadcast_client.close()
def on_broadcast_bind(self): # TODO move ALL binding code here
"""
Method called on binding to the server by broadcast. Override that method in order to add functionality.
"""
pass
def _process_connections(self):
while True:
events = self.selector.select(timeout=1)
for key, mask in events:
connection = key.data
if connection is not None:
try:
connection.process_events(mask)
except Exception as error:
logger.error(
"Exception {} occurred for {}! Resetting connection!"
.format(error, connection.addr))
self.server_connection._close()
self.connected = False
if isinstance(error, OSError):
if error.errno == errno.EINTR:
raise KeyboardInterrupt
try:
mapping_fds = self.selector.get_map().keys(
) # file descriptors
notifier_fd = messaging.NotifierSock().get_sock().fileno()
except (KeyError, RuntimeError) as e:
logger.error(
"Exception {} occurred when getting connections map!".
format(e))
logger.error(
"Connections changed during getting connections map, passing"
)
else:
notify_only = len(
mapping_fds) == 1 and notifier_fd in mapping_fds
if notify_only or not mapping_fds:
logger.warning("No active connections left!")
return
