def _create_packet(self):
packet_size = 0
packet_bytes = ""
sent_messages = []
self._log.debug('%d packets pending' % len(self._outgoing))
for message in self._outgoing:
if message.require_ack:
# a minimum resend delay is required for two reasons:
# 1. deadlock with a 0ms latency connection causes _do_write
# to never exit as _create_packet always returns data.
# 2. resending every 0ms is just plain stupid.
t = time.time()
resend_delay = max(self.MINIMUM_RESEND_DELAY_MS,
self._transport_latency)
self._log.debug('LSAT: %s' %
str(message.last_send_attempt_timestamp))
self._log.debug('l8nc: %s' % str(self._transport_latency))
self._log.debug('time: %s' % t)
self._log.debug('rsnd: %s' % resend_delay)
if message.last_send_attempt_timestamp is not None:
if ((message.last_send_attempt_timestamp + resend_delay) >=
t):
self._log.debug('Waiting for ack.')
continue
if packet_size + message.length <= self.MTU:
self._log.debug('Added data message into UDP packet')
packet_size += message.length
packet_bytes += message.message_bytes
message.last_send_attempt_timestamp = time.time()
sent_messages.append(message)
else:
self._log.debug('packet at MTU limit.')
for sent_message in sent_messages:
# Packets that require an ack are only removed
# from the outgoing list if an ack is received.
if not sent_message.require_ack:
self._log.info('Message %d doesnt require ack - removing' %
sent_message.message_id)
self._outgoing.remove(sent_message)
else:
self._log.info(
'Message %d requires ack - waiting for response' %
sent_message.message_id)
return packet_bytes
def __init__(self, parent=None, message_factory=None):
if message_factory is None:
self.message_factory = parent.message_factory
else:
self.message_factory = message_factory
self.parent = parent
self._last_receive_timestamp = time.time()
self._last_send_timestamp = time.time()
self._keep_alive_send_timestamp = time.time()
self._keep_alive_message_id = 0
# server: number of keepalives sent
# client: number of keepalives received
self._keepalive_count = 0
self._ping_id = 0
self._ping_send_timestamp = time.time()
self._ping_meter = PingSampler()
self.OnConnectRequestAccepted = Event()
self.OnConnectRequestRejected = Event()
self.OnConnectRequest = Event()
self.OnError = Event()
self.OnMessage = Event()
self.OnDisconnect = Event()
# Packet instances to be processed go in here
self._incoming_messages = []
# List of OutgoingMessages
self._outgoing = []
# In-order packet instances that have arrived early
self._incoming_out_of_sequence_messages = []
self._incoming_ordered_sequence_number = 0
self._outgoing_ordered_sequence_number = 1
self._outgoing_message_id = 0
self._recent_message_ids = []
# Metrics
self._in_bytes = 0
self._out_bytes = 0
self._in_packets = 0
self._out_packets = 0
self._in_messages = 0
self._out_messages = 0
'''
Default transport latency is high - This prevents spamming
of the network prior to obtaining a calculated latency.
'''
self._transport_latency = 0.3 # 0.1 = 100ms
def _send_ping(self):
self._ping_id += 1
if (self._ping_id > netshared.USHRT_MAX):
self._ping_id = 0
ping = self.message_factory.get_by_name('Ping')()
ping.id.value = self._ping_id
self.send_message(ping)
self._ping_send_timestamp = time.time()
def _send_keep_alive(self):
self._keep_alive_message_id += 1
if (self._keep_alive_message_id > netshared.USHRT_MAX):
self._keep_alive_message_id = 0
message = self.message_factory.get_by_name('KeepAliveRequest')()
message.id.value = self._keep_alive_message_id
self.send_message(message)
self._keep_alive_send_timestamp = time.time()
self._keepalive_count += 1
def __init__(self, parent=None, address=None):
self._connected = False
self.parent = parent
self._socket = parent.socket
self._address = address
self._last_receive_timestamp = time.time()
self._pending_disconnect = False
self.OnConnectRequest = Event()
self.OnDisconnect = Event()
self.OnError = Event()
self.OnMessage = Event()
self._connection = Service('Connection', {'parent': self})
self._connection.OnMessage += self._Connection_OnMessage
self._connection.OnDisconnect += self._Connection_OnDisconnect
self._connection.OnError += self._Connection_OnError
self._connection.OnConnectRequest += self._Connection_OnConnectRequest
def send_message(self, message, ordered=False, reliable=False):
'''
Send a message and specify any options for the send method used.
A message sent inOrder is implicitly sent as reliable.
message is an instance of a subclass of packets.BasePacket.
Returns the number of bytes added to the output queue for this
message (header + message).
'''
self._last_send_timestamp = time.time()
self._outgoing_message_id += 1
message_id = self._outgoing_message_id
if ordered:
self._outgoing_ordered_sequence_number += 1
inorder_sequence_number = self._outgoing_ordered_sequence_number
else:
inorder_sequence_number = 0
packet_flags = bitfield()
packet_flags[0] = int(ordered)
packet_flags[1] = int(reliable)
message_transport_header = struct.pack(
'!' + self.MESSAGE_TRANSPORT_HEADER, message_id,
inorder_sequence_number, int(packet_flags))
message_bytes = message.get_packet_bytes()
total_length = len(message_bytes) + len(message_transport_header)
self._out_bytes += total_length
self._add_message_bytes_to_output_list(
message_id, message_transport_header + message_bytes, ordered
or reliable)
self._log.debug('Packet data length = %s' % len(message_bytes))
self._log.debug('Header length = %s' % len(message_transport_header))
self._log.debug('Added %d byte %s packet in outgoing buffer' %
(total_length, message.__class__.__name__))
return total_length
class Connection(object):
MTU = 1400
MESSAGE_TRANSPORT_HEADER = 'HHB'
RECENT_MESSAGE_LIST_SIZE = 1000
MINIMUM_RESEND_DELAY_MS = 10 / 1000.0
_log = logging.getLogger('legume.Connection')
def __init__(self, parent=None, message_factory=None):
if message_factory is None:
self.message_factory = parent.message_factory
else:
self.message_factory = message_factory
self.parent = parent
self._last_receive_timestamp = time.time()
self._last_send_timestamp = time.time()
self._keep_alive_send_timestamp = time.time()
self._keep_alive_message_id = 0
# server: number of keepalives sent
# client: number of keepalives received
self._keepalive_count = 0
self._ping_id = 0
self._ping_send_timestamp = time.time()
self._ping_meter = PingSampler()
self.OnConnectRequestAccepted = Event()
self.OnConnectRequestRejected = Event()
self.OnConnectRequest = Event()
self.OnError = Event()
self.OnMessage = Event()
self.OnDisconnect = Event()
# Packet instances to be processed go in here
self._incoming_messages = []
# List of OutgoingMessages
self._outgoing = []
# In-order packet instances that have arrived early
self._incoming_out_of_sequence_messages = []
self._incoming_ordered_sequence_number = 0
self._outgoing_ordered_sequence_number = 1
self._outgoing_message_id = 0
self._recent_message_ids = []
# Metrics
self._in_bytes = 0
self._out_bytes = 0
self._in_packets = 0
self._out_packets = 0
self._in_messages = 0
self._out_messages = 0
'''
Default transport latency is high - This prevents spamming
of the network prior to obtaining a calculated latency.
'''
self._transport_latency = 0.3 # 0.1 = 100ms
@property
def out_buffer_bytes(self):
return sum([len(o.message_bytes) for o in self._outgoing])
@property
def latency(self):
return self._ping_meter.get_ping()
@property
def in_bytes(self):
return self._in_bytes
@property
def out_bytes(self):
return self._out_bytes
@property
def reorder_queue(self):
return len(self._incoming_out_of_sequence_messages)
@property
def keepalive_count(self):
return self._keepalive_count
# ------------- Public Methods -------------
def process_inbound_packet(self, data):
self._in_packets += 1
self._process_inbound_packet(data)
def update(self):
'''
Send any packets that are in the output buffer and read
any packets that have been received.
'''
try:
self.parent.do_read(self._on_socket_data)
except netshared.NetworkEndpointError, e:
self.raiseOnError('Connection reset by peer')
return
if self._ping_meter.has_estimate():
self._transport_latency = self._ping_meter.get_ping()
read_messages = self._update(self.parent._socket, self.parent._address)
if len(read_messages) != 0:
self._last_receive_timestamp = time.time()
for message in read_messages:
if self.message_factory.is_a(message, 'ConnectRequestAccepted'):
self.OnConnectRequestAccepted(self, None)
elif self.message_factory.is_a(message, 'ConnectRequestRejected'):
self.OnConnectRequestRejected(self, None)
elif self.message_factory.is_a(message, 'KeepAliveResponse'):
if (message.id.value == self._keep_alive_message_id):
self._ping_meter.add_sample(
(time.time() - self._keep_alive_send_timestamp) * 1000)
else:
self._log.warning('Received old keep-alive, discarding')
elif self.message_factory.is_a(message, 'KeepAliveRequest'):
self._keepalive_count += 1
response = self.message_factory.get_by_name(
'KeepAliveResponse')()
response.id.value = message.id.value
self.send_message(response)
elif self.message_factory.is_a(message, 'Pong'):
if (message.id.value == self._ping_id):
self._ping_meter.add_sample(
(time.time() - self._ping_send_timestamp) * 1000)
else:
self._log.warning('Received old Pong, discarding')
elif self.message_factory.is_a(message, 'Ping'):
self._send_pong(message.id.value)
elif self.message_factory.is_a(message, 'Disconnected'):
self._log.debug('Received `Disconnected` message')
self.OnDisconnect(self, None)
elif self.message_factory.is_a(message, 'MessageAck'):
self._process_message_ack(message.message_to_ack.value)
elif self.message_factory.is_a(message, 'ConnectRequest'):
# Unless the connection request is explicitly denied then
# a connection is made - OnConnectRequest may return None
# if no event handlers are bound.
accept = True
if (message.protocol.value != netshared.PROTOCOL_VERSION):
self._log.error('Invalid protocol version for client')
accept = False
if self.OnConnectRequest(self.parent, message) is False:
accept = False
if accept:
response = self.message_factory.get_by_name(
'ConnectRequestAccepted')
self.send_reliable_message(response())
else:
response = self.message_factory.get_by_name(
'ConnectRequestRejected')
self.send_reliable_message(response())
self.pendingDisconnect = True
else:
self.OnMessage(self, message)
if (time.time() > self._ping_send_timestamp + PING_REQUEST_FREQUENCY):
if self.parent.is_server:
self._keep_alive_send_timestamp = time.time()
self._send_ping()
if self.parent.is_server:
# Server sends keep alive requests...
if ((time.time() - self._keep_alive_send_timestamp) >
(self.parent.timeout / 2)):
self._send_keep_alive()
# though it will eventually give up...
if (time.time() -
self._last_receive_timestamp) > (self.parent.timeout):
self.OnError(self, 'Connection timed out')
else:
# ...Client waits for the connection to timeout
if (time.time() -
self._last_receive_timestamp) > (self.parent.timeout):
self._log.info('Connection has timed out')
self.OnError(self, 'Connection timed out')
from timeit import gc
total = list()
iterations = 10
# for i in range(iterations):
# start = timer()
# gc.enable()
# connect_graph2(G)
# end = timer()
# total.append((end - start))
# print('connect graph: ',np.mean(total), np.std(total))
# total = list()
for i in range(iterations):
# start = timer()
# gc.enable()
timing.log('Connect graph started')
time = timing.time()
connect_graph3(G)
timing.log('Connect graph ended', timing.time()-time)
# plt.plot(times0)
# plt.plot(times1)
# plt.plot(times11)
# plt.plot(times2)
# plt.plot(times3)
# plt.plot(times4)
# plt.plot(times5)
print('complete while loop ocurred ', len(times0), ' with mean ', np.mean(times0), ' total cost ', len(times0)*np.mean(times0))
print('get_oposition_points_by_distance ocurred ', len(times1), ' with mean ', np.mean(times1), ' total cost ', len(times1)*np.mean(times1))
print('get_triangles_to_look ocurred ', len(times11), ' with mean ', np.mean(times11), ' total cost ', len(times11)*np.mean(times11))
print('for candidate loop ocurred ', len(times2), ' with mean ', np.mean(times2), ' total cost ', len(times2)*np.mean(times2))
print('create new_triangle ocurred ', len(times3), ' with mean ', np.mean(times3), ' total cost ', len(times3)*np.mean(times3))
print('check_triangle_intersection ocurred ', len(times4), ' with mean ', np.mean(times4), ' total cost ', len(times4)*np.mean(times4))
gyro_slipping = qt.quaternion_rotate(q_pitch1, gyro[:, i])
slipping_one[i] = one_wheel_slip_detection(
encoder.r, encoder.d, np.array([enc_meas[-1, i], enc_meas[-2, i]]),
gyro_slipping)
# Data for Sensor data plots
theta = x2[2, i]
q_pitch = qt.quaternion(theta, [0, 1, 0])
psi = x2[4, i]
q_yaw = qt.quaternion(psi, [0, 0, 1])
q_s = qt.multiply(q_yaw, q_pitch)
gyro_s[:, i] = qt.quaternion_rotate(q_pitch, gyro[:, i])
acc_s[:, i] = qt.quaternion_rotate(q_pitch, acceleration[:, i])
# Orientation Estimation
t_ori = timing.time()
R_IMU_b = np.identity(3)
acc = R_IMU_b @ acceleration[:, i]
gyr = R_IMU_b @ gyro[:, i]
V = np.identity(4) * 0.1
W = np.identity(3) * 1000000
xh[:, i], Ph = quaternionestimation.ekf(xh[:, i - 1], gyr, acc, head_enc,
V, W, Ph, 9.81, robot.Ts)
heading[:, i] = qt.get_heading(xh[:, i])
euler[:, i] = qt.euler_from_q(xh[:, i], 'zyx', True)
time_orientation[i] = timing.time() - t_ori
# Position Estimation
t_pos = timing.time()
