def async_run(coroutines):
future = Future()
outcomes = [None] * len(coroutines)
for i, co in enumerate(coroutines):
try:
next(co)
except StopIteration as ex:
outcomes[i] = ex.value
except Exception as ex:
future.set_exception(ex)
return future, None
if all(value is not None for value in outcomes):
future.set_result(outcomes)
return future, None
def _resume(to_completion=False):
nonlocal outcomes
if future.done():
raise RuntimeError("'async_run' done already")
for i, co in enumerate(coroutines):
if outcomes[i] is None:
try:
co.send(to_completion)
except StopIteration as ex:
outcomes[i] = ex.value
except Exception as ex:
future.set_exception(ex)
return
if to_completion or all(value is not None for value in outcomes):
future.set_result(outcomes)
return future, _resume
def test_add_done_callback_invokes_callback(self):
called = False
def cb(fut):
nonlocal called
called = True
f = Future()
f.set_result('Anything')
f.add_done_callback(cb)
assert called
def main():
done = Future()
ros = RosBackend(done)
rosthread = Thread(target=ros.spin_until_future_complete)
rosthread.start()
gui = TkGUI()
gui.start()
print("shutting down ros...")
done.set_result(True)
rosthread.join()
def test_await(self):
f = Future()
async def coro():
nonlocal f
return await f
c = coro()
c.send(None)
f.set_result('Sentinel Result')
try:
c.send(None)
except StopIteration as e:
self.assertEqual('Sentinel Result', e.value)
def wait_for_message(self,
topic,
msg_type,
timeout=None,
qos_profile=1):
"""
Wait for one message from topic.
This will create a new subcription to the topic, receive one message, then unsubscribe.
Do not call this method in a callback or a deadlock may occur.
"""
s = None
try:
future = Future()
s = self.new_subscription(msg_type,
topic,
lambda msg: future.set_result(msg),
qos_profile=qos_profile)
rclpy.spin_until_future_complete(self, future, self.executor,
timeout)
finally:
if s is not None:
self.destroy_subscription(s)
return future.result()
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 test_set_result_schedules_callbacks(self):
executor = DummyExecutor()
f = Future(executor=executor)
f.add_done_callback(lambda f: None)
f.set_result('Anything')
self.assertTrue(executor.done_callbacks)
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())
class NavNode(rclpy.node.Node):
def __init__(self, x, y, theta, timeout):
super().__init__('nav_demo')
#self.create_timer(.1, self.timer_callback)
latching_qos = QoSProfile(depth=1,
durability=QoSDurabilityPolicy.
RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL)
self.create_subscription(OccupancyGrid,
'map',
self.map_callback,
qos_profile=latching_qos)
# Create the action client and wait until it is active
#self.ac = ActionClient(self, NavigateToPose, '/NavigateToPose')
self.ac = ActionClient(self, NavigateToPose, '/navigate_to_pose')
# Set up the goal message
self.goal = NavigateToPose.Goal()
self.goal.pose.header.frame_id = 'map' # SEEMS TO BE IGNORED!
self.goal.pose.pose.position.x = x
self.goal.pose.pose.position.y = y
# We need to convert theta to a quaternion....
quaternion = transformations.quaternion_from_euler(0, 0, theta, 'rxyz')
self.goal.pose.pose.orientation.x = quaternion[0]
self.goal.pose.pose.orientation.y = quaternion[1]
self.goal.pose.pose.orientation.z = quaternion[2]
self.goal.pose.pose.orientation.w = quaternion[3]
self.timeout = timeout
def send_goal(self):
self.get_logger().info("WAITING FOR NAVIGATION SERVER...")
self.ac.wait_for_server()
self.get_logger().info("NAVIGATION SERVER AVAILABLE...")
self.get_logger().info("SENDING GOAL TO NAVIGATION SERVER...")
self.start_time = time.time()
self.cancel_future = None
self.is_done = False
self.map = None
self.goal_future = self.ac.send_goal_async(self.goal)
self.create_timer(.1, self.timer_callback)
self.future_event = Future()
return self.future_event
def map_callback(self, map_msg):
"""Process the map message. This doesn't really do anything useful, it is
purely intended as an illustration of the Map class.
"""
if self.map is None: # No need to do this every time map is published.
self.map = map_utils.Map(map_msg)
# Use numpy to calculate some statistics about the map:
total_cells = self.map.width * self.map.height
pct_occupied = np.count_nonzero(
self.map.grid == 100) / total_cells * 100
pct_unknown = np.count_nonzero(
self.map.grid == -1) / total_cells * 100
pct_free = np.count_nonzero(self.map.grid == 0) / total_cells * 100
map_str = "Map Statistics: occupied: {:.1f}% free: {:.1f}% unknown: {:.1f}%"
self.get_logger().info(
map_str.format(pct_occupied, pct_free, pct_unknown))
# Here is how to access map cells to see if they are free:
# x = 2.06
# y = -1.2
x = self.goal.pose.pose.position.x
y = self.goal.pose.pose.position.y
val = self.map.get_cell(x, y)
if val == 100:
free = "occupied"
elif val == 0:
free = "free"
else:
free = "unknown"
self.get_logger().info(
"HEY! Map position ({:.2f}, {:.2f}) is {}".format(x, y, free))
def done(self):
""" This allows the node to act as a future object. """
return self.is_done
def timer_callback(self):
""" Periodically check in on the progress of navigation. """
if not self.goal_future.done():
self.get_logger().info("NAVIGATION GOAL NOT YET ACCEPTED")
elif self.cancel_future is not None: # We've cancelled and are waiting for ack.
if self.cancel_future.done():
self.ac.destroy()
self.is_done = True
self.future_event.set_result(None)
else:
if self.goal_future.result().status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info(
"NAVIGATION SERVER REPORTS SUCCESS. EXITING!")
self.ac.destroy()
self.is_done = True
self.future_event.set_result(None)
if self.goal_future.result().status == GoalStatus.STATUS_ABORTED:
self.get_logger().info(
"NAVIGATION SERVER HAS ABORTED. EXITING!")
self.ac.destroy()
self.is_done = True
self.future_event.set_result(None)
elif time.time() - self.start_time > self.timeout:
self.get_logger().info("TAKING TOO LONG. CANCELLING GOAL!")
self.cancel_future = self.goal_future.result(
).cancel_goal_async()
self.future_event.set_result(None)
class EchoVerb(VerbExtension):
"""Output messages from a topic."""
def add_arguments(self, parser, cli_name):
add_strategy_node_arguments(parser)
arg = parser.add_argument(
'topic_name',
help="Name of the ROS topic to listen to (e.g. '/chatter')")
arg.completer = TopicNameCompleter(
include_hidden_topics_key='include_hidden_topics')
parser.add_argument(
'message_type',
nargs='?',
help="Type of the ROS message (e.g. 'std_msgs/msg/String')")
parser.add_argument(
'--qos-profile',
choices=rclpy.qos.QoSPresetProfiles.short_keys(),
help=
'Quality of service preset profile to subscribe with (default: {})'
.format(default_profile_str))
default_profile = rclpy.qos.QoSPresetProfiles.get_from_short_key(
default_profile_str)
parser.add_argument('--qos-depth',
metavar='N',
type=int,
help='Queue size setting to subscribe with '
'(overrides depth value of --qos-profile option)')
parser.add_argument(
'--qos-history',
choices=rclpy.qos.QoSHistoryPolicy.short_keys(),
help='History of samples setting to subscribe with '
'(overrides history value of --qos-profile option, default: {})'.
format(default_profile.history.short_key))
parser.add_argument(
'--qos-reliability',
choices=rclpy.qos.QoSReliabilityPolicy.short_keys(),
help='Quality of service reliability setting to subscribe with '
'(overrides reliability value of --qos-profile option, default: '
'Automatically match existing publishers )')
parser.add_argument(
'--qos-durability',
choices=rclpy.qos.QoSDurabilityPolicy.short_keys(),
help='Quality of service durability setting to subscribe with '
'(overrides durability value of --qos-profile option, default: '
'Automatically match existing publishers )')
parser.add_argument(
'--csv',
action='store_true',
help=
('Output all recursive fields separated by commas (e.g. for '
'plotting). '
'If --include-message-info is also passed, the following fields are prepended: '
'source_timestamp, received_timestamp, publication_sequence_number,'
' reception_sequence_number.'))
parser.add_argument(
'--field',
type=str,
default=None,
help='Echo a selected field of a message. '
"Use '.' to select sub-fields. "
'For example, to echo the position field of a nav_msgs/msg/Odometry message: '
"'ros2 topic echo /odom --field pose.pose.position'",
)
parser.add_argument(
'--full-length',
'-f',
action='store_true',
help='Output all elements for arrays, bytes, and string with a '
"length > '--truncate-length', by default they are truncated "
"after '--truncate-length' elements with '...''")
parser.add_argument(
'--truncate-length',
'-l',
type=unsigned_int,
default=DEFAULT_TRUNCATE_LENGTH,
help='The length to truncate arrays, bytes, and string to '
'(default: %d)' % DEFAULT_TRUNCATE_LENGTH)
parser.add_argument('--no-arr',
action='store_true',
help="Don't print array fields of messages")
parser.add_argument('--no-str',
action='store_true',
help="Don't print string fields of messages")
parser.add_argument(
'--flow-style',
action='store_true',
help=
'Print collections in the block style (not available with csv format)'
)
parser.add_argument('--lost-messages',
action='store_true',
help='DEPRECATED: Does nothing')
parser.add_argument('--no-lost-messages',
action='store_true',
help="Don't report when a message is lost")
parser.add_argument('--raw',
action='store_true',
help='Echo the raw binary representation')
parser.add_argument('--filter',
dest='filter_expr',
help='Python expression to filter messages that '
'are printed. Expression can use Python builtins '
'as well as m (the message).')
parser.add_argument(
'--once',
action='store_true',
help='Print the first message received and then exit.')
parser.add_argument('--include-message-info',
'-i',
action='store_true',
help='Shows the associated message info.')
def choose_qos(self, node, args):
if (args.qos_profile is not None or args.qos_reliability is not None
or args.qos_durability is not None
or args.qos_depth is not None or args.qos_history is not None):
if args.qos_profile is None:
args.qos_profile = default_profile_str
return qos_profile_from_short_keys(
args.qos_profile,
reliability=args.qos_reliability,
durability=args.qos_durability,
depth=args.qos_depth,
history=args.qos_history)
qos_profile = QoSPresetProfiles.get_from_short_key(default_profile_str)
reliability_reliable_endpoints_count = 0
durability_transient_local_endpoints_count = 0
pubs_info = node.get_publishers_info_by_topic(args.topic_name)
publishers_count = len(pubs_info)
if publishers_count == 0:
return qos_profile
for info in pubs_info:
if (info.qos_profile.reliability == QoSReliabilityPolicy.RELIABLE):
reliability_reliable_endpoints_count += 1
if (info.qos_profile.durability ==
QoSDurabilityPolicy.TRANSIENT_LOCAL):
durability_transient_local_endpoints_count += 1
# If all endpoints are reliable, ask for reliable
if reliability_reliable_endpoints_count == publishers_count:
qos_profile.reliability = QoSReliabilityPolicy.RELIABLE
else:
if reliability_reliable_endpoints_count > 0:
print('Some, but not all, publishers are offering '
'QoSReliabilityPolicy.RELIABLE. Falling back to '
'QoSReliabilityPolicy.BEST_EFFORT as it will connect '
'to all publishers')
qos_profile.reliability = QoSReliabilityPolicy.BEST_EFFORT
# If all endpoints are transient_local, ask for transient_local
if durability_transient_local_endpoints_count == publishers_count:
qos_profile.durability = QoSDurabilityPolicy.TRANSIENT_LOCAL
else:
if durability_transient_local_endpoints_count > 0:
print('Some, but not all, publishers are offering '
'QoSDurabilityPolicy.TRANSIENT_LOCAL. Falling back to '
'QoSDurabilityPolicy.VOLATILE as it will connect '
'to all publishers')
qos_profile.durability = QoSDurabilityPolicy.VOLATILE
return qos_profile
def main(self, *, args):
if args.lost_messages:
print(
"WARNING: '--lost-messages' is deprecated; lost messages are reported by default",
file=sys.stderr)
self.csv = args.csv
# Validate field selection
self.field = args.field
if self.field is not None:
self.field = list(filter(None, self.field.split('.')))
if not self.field:
raise RuntimeError(f"Invalid field value '{args.field}'")
self.truncate_length = args.truncate_length if not args.full_length else None
self.no_arr = args.no_arr
self.no_str = args.no_str
self.flow_style = args.flow_style
self.filter_fn = None
if args.filter_expr:
self.filter_fn = _expr_eval(args.filter_expr)
self.future = None
if args.once:
self.future = Future()
self.include_message_info = args.include_message_info
with NodeStrategy(args) as node:
qos_profile = self.choose_qos(node, args)
if args.message_type is None:
message_type = get_msg_class(node,
args.topic_name,
include_hidden_topics=True)
else:
try:
message_type = get_message(args.message_type)
except (AttributeError, ModuleNotFoundError, ValueError):
raise RuntimeError('The passed message type is invalid')
if message_type is None:
raise RuntimeError(
'Could not determine the type for the passed topic')
self.subscribe_and_spin(node, args.topic_name, message_type,
qos_profile, args.no_lost_messages,
args.raw)
def subscribe_and_spin(
self,
node: Node,
topic_name: str,
message_type: MsgType,
qos_profile: QoSProfile,
no_report_lost_messages: bool,
raw: bool,
) -> Optional[str]:
"""Initialize a node with a single subscription and spin."""
event_callbacks = None
if not no_report_lost_messages:
event_callbacks = SubscriptionEventCallbacks(
message_lost=_message_lost_event_callback)
try:
node.create_subscription(message_type,
topic_name,
self._subscriber_callback,
qos_profile,
event_callbacks=event_callbacks,
raw=raw)
except UnsupportedEventTypeError:
assert not no_report_lost_messages
node.create_subscription(message_type,
topic_name,
self._subscriber_callback,
qos_profile,
event_callbacks=None,
raw=raw)
if self.future is not None:
rclpy.spin_until_future_complete(node, self.future)
else:
rclpy.spin(node)
def _subscriber_callback(self, msg, info):
submsg = msg
if self.field is not None:
for field in self.field:
try:
submsg = getattr(submsg, field)
except AttributeError as ex:
raise RuntimeError(
f"Invalid field '{'.'.join(self.field)}': {ex}")
# Evaluate the current msg against the supplied expression
if self.filter_fn is not None and not self.filter_fn(submsg):
return
if self.future is not None:
self.future.set_result(True)
if not hasattr(submsg, '__slots__'):
# raw
if self.include_message_info:
print('---Got new message, message info:---')
print(info)
print('---Message data:---')
print(submsg, end='\n---\n')
return
if self.csv:
to_print = message_to_csv(submsg,
truncate_length=self.truncate_length,
no_arr=self.no_arr,
no_str=self.no_str)
if self.include_message_info:
to_print = f'{",".join(str(x) for x in info.values())},{to_print}'
print(to_print)
return
# yaml
if self.include_message_info:
print(yaml.dump(info), end='---\n')
print(message_to_yaml(submsg,
truncate_length=self.truncate_length,
no_arr=self.no_arr,
no_str=self.no_str,
flow_style=self.flow_style),
end='---\n')
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
