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