def spin_until_future_complete(self,
future: Future,
timeout_sec: float = None) -> None:
"""Execute callbacks until a given future is done or a timeout occurs."""
# Make sure the future wakes this executor when it is done
future.add_done_callback(lambda x: self.wake())
if timeout_sec is None or timeout_sec < 0:
while self._context.ok(
) and not future.done() and not self._is_shutdown:
self.spin_once_until_future_complete(future, timeout_sec)
else:
start = time.monotonic()
end = start + timeout_sec
timeout_left = timeout_sec
while self._context.ok(
) and not future.done() and not self._is_shutdown:
self.spin_once_until_future_complete(future, timeout_left)
now = time.monotonic()
if now >= end:
return
timeout_left = end - now
def _test_statistic_publication(self, topic_name: str,
expected_nodes: Iterable[str]):
future = Future()
message_counter = Counter()
lock = Lock()
# arbitrary choice, just tells if it's working for a little while
expected_messages_per_node = 3
# we are receiving stats every 10 seconds, so this should pass in 30s
timeout_sec = 180
def message_callback(msg):
node_name = '/' + msg.measurement_source_name
with lock:
message_counter[node_name] += 1
if all(message_counter[node] >= expected_messages_per_node
for node in expected_nodes):
print('Successfully received all expected messages')
future.set_result(True)
sub = self.node.create_subscription(MetricsMessage,
topic_name,
message_callback,
qos_profile=10)
rclpy.spin_until_future_complete(self.node,
future,
timeout_sec=timeout_sec)
self.assertTrue(
future.done(),
f'Timed out, received message count: {message_counter}')
self.node.destroy_subscription(sub)
def test_executor_spin_until_future_complete_do_not_wait(self):
self.assertIsNotNone(self.node.handle)
executor = SingleThreadedExecutor(context=self.context)
executor.add_node(self.node)
def timer_callback():
pass
timer = self.node.create_timer(0.003, timer_callback)
# Do not wait timeout_sec = 0
future = Future()
self.assertFalse(future.done())
executor.spin_until_future_complete(future=future, timeout_sec=0)
self.assertFalse(future.done())
timer.cancel()
def spin_until_future_complete(self, future: Future, timeout_sec: float = None) -> None:
"""Execute callbacks until a given future is done or a timeout occurs."""
if timeout_sec is None or timeout_sec < 0:
while self._context.ok() and not future.done():
self.spin_once(timeout_sec=timeout_sec)
else:
start = time.monotonic()
end = start + timeout_sec
timeout_left = timeout_sec
while self._context.ok() and not future.done():
self.spin_once(timeout_sec=timeout_left)
now = time.monotonic()
if now >= end:
return
timeout_left = end - now
def test_excutor_spin_until_future_complete_do_not_wait(self):
self.assertIsNotNone(self.node.handle)
executor = SingleThreadedExecutor(context=self.context)
executor.add_node(self.node)
def timer_callback():
pass
timer = self.node.create_timer(0.003, timer_callback)
# Do not wait timeout_sec = 0
future = Future()
self.assertFalse(future.done())
start = time.monotonic()
executor.spin_until_future_complete(future=future, timeout_sec=0)
end = time.monotonic()
self.assertAlmostEqual(end - start, 0.0, delta=0.01)
self.assertFalse(future.done())
timer.cancel()
def test_executor_spin_until_future_complete_timeout(self):
self.assertIsNotNone(self.node.handle)
executor = SingleThreadedExecutor(context=self.context)
executor.add_node(self.node)
def timer_callback():
pass
timer = self.node.create_timer(0.003, timer_callback)
# Timeout
future = Future()
self.assertFalse(future.done())
start = time.monotonic()
executor.spin_until_future_complete(future=future, timeout_sec=0.1)
end = time.monotonic()
# Nothing is ever setting the future, so this should have waited
# at least 0.1 seconds.
self.assertGreaterEqual(end - start, 0.1)
self.assertFalse(future.done())
timer.cancel()
class TestConditionSubscriberNode(Node):
"""Subscribes to condition messages for the purpose of testing"""
__msg_received: Optional[Future] = None
def __init__(self):
super().__init__('test_cond_sub_node')
self.create_subscription(
Condition,
'acf_process_cell_is_sheilded',
callback=lambda msg: (
self.get_logger().info("recieved {}".format(msg)),
#If a future has been created, set the result as the message, otherwise discard the message
self.__msg_received.set_result(msg)
if self.__msg_received is not None else
(self.get_logger().info("not expecting message"))),
qos_profile=latching_qos)
def expect_message_within_time(self, timeout, expected_value) -> bool:
"""blocking fucntion"""
expected_msg = Condition()
expected_msg.condition = int(expected_value)
self.__msg_received = Future(executor=self.executor)
timeout_timer = self.create_timer(
timer_period_sec=timeout,
callback=lambda:
(self.get_logger().info("Timeout!"),
self.__msg_received.set_result(False), timeout_timer.cancel()))
#Wait for either timeout or new message to arrive
while not self.__msg_received.done():
pass
#Return true or false based on result of future
if self.__msg_received.result == False:
return False
elif type(self.__msg_received) is not Condition:
self.get_logger().info("Wrong message type stored in future")
return False
else:
pass
def main():
parser = argparse.ArgumentParser(
description='CLI tool for checking message equality')
parser.add_argument('--name',
required=True,
help='The name of the message to listen for')
parser.add_argument('--type',
required=True,
help='The type of the message')
parser.add_argument('--expected',
required=True,
help='The expected values (YAML format)')
parser.add_argument('--timeout',
default=None,
type=nonnegative_int,
help='The amount of time to wait for a message')
args = parser.parse_args()
if len(sys.argv) == 1:
parser.print_help(sys.stdout)
sys.exit(0)
values_dictionary = yaml.safe_load(args.expected)
if not isinstance(values_dictionary, dict):
return 'The passed value needs to be a dictionary in YAML format'
rclpy.init()
future = Future()
node = MessageEqualityTesterNode(args.name, args.type, values_dictionary,
future)
rclpy.spin_until_future_complete(node, future, timeout_sec=args.timeout)
if not future.done():
node.get_logger().error("Timeout after " + str(args.timeout) +
" seccond: No message on \"" + args.name +
"\" topic")
node.destroy_node()
rclpy.shutdown()
def test_excutor_spin_until_future_complete_future_done(self):
self.assertIsNotNone(self.node.handle)
executor = SingleThreadedExecutor(context=self.context)
executor.add_node(self.node)
def timer_callback():
pass
timer = self.node.create_timer(0.003, timer_callback)
def set_future_result(future):
time.sleep(0.1)
future.set_result('finished')
# Future complete timeout_sec > 0
future = Future()
self.assertFalse(future.done())
t = threading.Thread(target=lambda: set_future_result(future))
t.start()
start = time.monotonic()
executor.spin_until_future_complete(future=future, timeout_sec=0.2)
end = time.monotonic()
self.assertAlmostEqual(end - start, 0.1, delta=0.01)
self.assertTrue(future.done())
self.assertEqual(future.result(), 'finished')
# Future complete timeout_sec = None
future = Future()
self.assertFalse(future.done())
t = threading.Thread(target=lambda: set_future_result(future))
t.start()
start = time.monotonic()
executor.spin_until_future_complete(future=future, timeout_sec=None)
end = time.monotonic()
self.assertAlmostEqual(end - start, 0.1, delta=0.01)
self.assertTrue(future.done())
self.assertEqual(future.result(), 'finished')
# Future complete timeout < 0
future = Future()
self.assertFalse(future.done())
t = threading.Thread(target=lambda: set_future_result(future))
t.start()
start = time.monotonic()
executor.spin_until_future_complete(future=future, timeout_sec=-1)
end = time.monotonic()
self.assertAlmostEqual(end - start, 0.1, delta=0.01)
self.assertTrue(future.done())
self.assertEqual(future.result(), 'finished')
timer.cancel()
def test_set_exception(self):
f = Future()
f.set_exception('Sentinel Exception')
self.assertEqual('Sentinel Exception', f.exception())
self.assertTrue(f.done())
def test_set_result(self):
f = Future()
f.set_result('Sentinel Result')
self.assertEqual('Sentinel Result', f.result())
self.assertTrue(f.done())
def test_done(self):
f = Future()
self.assertFalse(f.done())
f.set_result(None)
self.assertTrue(f.done())
def spin_until_future_complete(self, future: Future) -> None:
"""Execute callbacks until a given future is done."""
while self._context.ok() and not future.done():
self.spin_once()
class TimeVaryingCommunicator(BestEffortCommunicator):
def __init__(self,
agent_id,
size,
in_neighbors,
out_neighbors=None,
synchronous_mode=True,
differentiated_topics=False):
# initialize additional variables for synchronous communication
self.future = None
self.synchronous_mode = synchronous_mode
self.executor = SingleThreadedExecutor() if synchronous_mode else None
# continue initialization
super().__init__(agent_id, size, in_neighbors, out_neighbors,
differentiated_topics)
# QoS profile for reliable communication
def _getQoSProfile(self):
profile = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_ALL)
profile.reliability = QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE
profile.durability = QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL
profile.liveliness = QoSLivelinessPolicy.RMW_QOS_POLICY_LIVELINESS_AUTOMATIC
profile.deadline = Duration()
profile.lifespan = Duration()
return profile
def _get_callback_groups(self, in_neighbors):
return {j: AuthorizationCallbackGroup() for j in in_neighbors}
def neighbors_receive(self, neighbors, stop_event: Event = None):
"""Receive data from neighbors (waits until data are received from all neighbors)
Args:
neighbors (list): list of neighbors
Returns:
dict: dict containing received data associated to each neighbor in neighbors
"""
if not self.synchronous_mode:
raise Exception(
"Cannot perform synchronous receive because communicator is in asynchronous mode"
)
for j in neighbors:
if j not in self.subscriptions:
raise RuntimeError(
"Cannot receive from {} since it is not an in-neighbor".
format(j))
self.neighbors = neighbors
self.received_data = {} # initialize dictionary of received data
self.future = Future()
for k in self.callback_groups:
self.callback_groups[k].give_authorization()
# receive a single message from all neighbors (check Event every 'timeout' seconds)
timeout = 0.2 if stop_event is not None else None
while not self.future.done():
rclpy.spin_until_future_complete(self.node,
self.future,
executor=self.executor,
timeout_sec=timeout)
# check if external event has been set
if stop_event is not None and stop_event.is_set():
# remove pending messages from topics
for k in self.callback_groups:
self.callback_groups[k].give_authorization(permanent=True)
self.synchronous_mode = False
self.neighbors_receive_asynchronous(neighbors)
self.synchronous_mode = True
for k in self.callback_groups:
self.callback_groups[k].draw_authorization()
break
return self.received_data
def neighbors_receive_asynchronous(self, neighbors):
if self.synchronous_mode:
raise Exception(
"Cannot perform asynchronous receive because communicator is in synchronous mode"
)
return super().neighbors_receive_asynchronous(neighbors)
def _subscription_callback(self, msg, node):
# perform actual callback and check status
if super()._subscription_callback(msg, node):
# notify reception from all neighbors if necessary
if self.synchronous_mode and len(self.received_data) == len(
self.neighbors):
self.future.set_result(1)
def neighbors_exchange(self,
send_obj,
in_neighbors,
out_neighbors,
dict_neigh,
stop_event: Event = None):
"""exchange information (synchronously) with neighbors
Args:
send_obj (any): object to send
in_neighbors (list): list of in-neighbors
out_neighbors (list): list of out-neighbors
dict_neigh: True if send_obj contains a dictionary with different objects for each neighbor
Returns:
dict: dict containing received data associated to each neighbor in in-neighbors
"""
if not dict_neigh:
self.neighbors_send(send_obj, out_neighbors)
else:
if not self.differentiated_topics:
raise RuntimeError(
'Communicator must be initialized with differentiated topics in order to use dict_neigh=True'
)
for j in out_neighbors:
self.neighbors_send(send_obj[j], [j])
data = self.neighbors_receive(in_neighbors, stop_event)
return data
