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 _cancel_goal_async(self, goal_handle): """ Send a cancel request for an active goal and asynchronously get the result. :param goal_handle: Handle to the goal to cancel. :type goal_handle: :class:`ClientGoalHandle` :return: a Future instance that completes when the cancel request has been processed. :rtype: :class:`rclpy.task.Future` instance """ if not isinstance(goal_handle, ClientGoalHandle): raise TypeError( 'Expected type ClientGoalHandle but received {}'.format( type(goal_handle))) cancel_request = CancelGoal.Request() cancel_request.goal_info.goal_id = goal_handle.goal_id sequence_number = self._client_handle.send_cancel_request( cancel_request) if sequence_number in self._pending_cancel_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending cancel request'.format( sequence_number)) future = Future() self._pending_cancel_requests[sequence_number] = future future.add_done_callback(self._remove_pending_cancel_request) # Add future so executor is aware self.add_future(future) return future
def test_orbslam2_buffer(): # bag_wrapper(wrap_node, rosbag_location, kwargs): rosbag_location = "./test/testing_resources/rosbag/test.bag" pose_args = {"visualize": False} orbslam2PoseWrapped = bag_wrapper(Orbslam2Pose, rosbag_location, pose_args) args = { "wrap_node": Orbslam2Pose, "rosbag_location": rosbag_location, "kwargs": pose_args, } p = Process( target=run_node, args=( orbslam2PoseWrapped, args, ), ) p.start() future = Future() bufferTester = BufferTester(future) rclpy.spin_until_future_complete(bufferTester, future) bufferTester.destroy_node() os.kill(p.pid, signal.SIGKILL) rclpy.shutdown() assert future.result() == "Pass"
def call_async(self, request: SrvTypeRequest) -> Future: """ Make a service request and asyncronously get the result. :param request: The service request. :return: A future that completes when the request does. :raises: TypeError if the type of the passed request isn't an instance of the Request type of the provided service when the client was constructed. """ if not isinstance(request, self.srv_type.Request): raise TypeError() with self._lock: with self.handle: sequence_number = self.__client.send_request(request) if sequence_number in self._pending_requests: raise RuntimeError( f'Sequence ({sequence_number}) conflicts with pending request' ) future = Future() self._pending_requests[sequence_number] = future future.add_done_callback(self.remove_pending_request) return future
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 _get_result_async(self, goal_handle): """ Request the result for an active goal asynchronously. :param goal_handle: Handle to the goal to cancel. :type goal_handle: :class:`ClientGoalHandle` :return: a Future instance that completes when the get result request has been processed. :rtype: :class:`rclpy.task.Future` instance """ if not isinstance(goal_handle, ClientGoalHandle): raise TypeError( 'Expected type ClientGoalHandle but received {}'.format(type(goal_handle))) result_request = self._action_type.Impl.GetResultService.Request() result_request.goal_id = goal_handle.goal_id sequence_number = _rclpy_action.rclpy_action_send_result_request( self._client_handle, result_request) if sequence_number in self._pending_result_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending result request'.format(sequence_number)) future = Future() self._pending_result_requests[sequence_number] = future future.add_done_callback(self._remove_pending_result_request) # Add future so executor is aware self.add_future(future) return future
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()
def test_bag(): rosbag_location = "./test/testing_resources/rosbag/test_short.bag" p = Process( target=play_rosbag, args=( rosbag_location, False, "-l --topics /terrasentia/usb_cam_node/image_raw", ), ) p.start() future = Future() bag_tester = BagTester(future, timeout=5.0) rclpy.spin_until_future_complete(bag_tester, future) bag_tester.destroy_node() # kills test.bag kill_testbag_cmd = ( "export PYTHONPATH= && . /opt/ros/melodic/setup.sh && rosnode list " + "| grep play | xargs rosnode kill" ) subprocess.Popen( kill_testbag_cmd, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT, shell=True, ) # DO NOT FORGET TO START ROSBRIDGE AND ROSCORE!!! assert future.result() == "Pass"
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_await_exception(self): f = Future() async def coro(): nonlocal f return await f c = coro() c.send(None) f.set_exception(RuntimeError('test exception')) with self.assertRaises(RuntimeError): c.send(None)
def main(args=None): rclpy.init(args=args) node = SkibotNode() executor = MultiThreadedExecutor(num_threads=4) executor.add_node(node) future = Future() future.add_done_callback(lambda fut: print("Future is done")) executor.spin_until_future_complete(future) node.destroy_node() rclpy.shutdown()
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 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 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 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 send_goal_async(self, goal, feedback_callback=None, goal_uuid=None): """ Send a goal and asynchronously get the result. The result of the returned Future is set to a ClientGoalHandle when receipt of the goal is acknowledged by an action server. :param goal: The goal request. :type goal: action_type.Goal :param feedback_callback: Callback function for feedback associated with the goal. :type feedback_callback: function :param goal_uuid: Universally unique identifier for the goal. If None, then a random UUID is generated. :type: unique_identifier_msgs.UUID :return: a Future instance to a goal handle that completes when the goal request has been accepted or rejected. :rtype: :class:`rclpy.task.Future` instance :raises: TypeError if the type of the passed goal isn't an instance of the Goal type of the provided action when the service was constructed. """ if not isinstance(goal, self._action_type.Goal): raise TypeError() request = self._action_type.Impl.SendGoalService.Request() request.goal_id = self._generate_random_uuid( ) if goal_uuid is None else goal_uuid request.goal = goal sequence_number = _rclpy_action.rclpy_action_send_goal_request( self._client_handle, request) if sequence_number in self._pending_goal_requests: raise RuntimeError( 'Sequence ({}) conflicts with pending goal request'.format( sequence_number)) if feedback_callback is not None: # TODO(jacobperron): Move conversion function to a general-use package goal_uuid = bytes(request.goal_id.uuid) self._feedback_callbacks[goal_uuid] = feedback_callback future = Future() self._pending_goal_requests[sequence_number] = future self._sequence_number_to_goal_id[sequence_number] = request.goal_id future.add_done_callback(self._remove_pending_goal_request) # Add future so executor is aware self.add_future(future) return future
def test_top_map(): try: rosbag_location = "./test/testing_resources/rosbag/test_long.bag" p2 = Process( target=play_rosbag, args=(rosbag_location, False, ""), ) p2.start() future = Future() topMapTester = TopMapTester(future, timeout=10) rclpy.spin_until_future_complete(topMapTester, future) except Exception as e: raise (e) else: topMapTester.destroy_node() assert topMapTester.future.result() == "Pass" finally: # kills test.bag kill_testbag_cmd = ( "export PYTHONPATH= && . /opt/ros/melodic/setup.sh && rosnode list " + "| grep play | xargs rosnode kill" ) subprocess.Popen( kill_testbag_cmd, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT, shell=True, ) os.kill(p2.pid, signal.SIGKILL)
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 check_for_statistic_publications(expected_nodes: List, num_msgs: int, expected_topic: str) -> None: """ Check that all nodes publish a statistics message. This will suceed fast if all expected messages were published, otherwise timeout (default TIMEOUT_SECONDS) if any publishers have not been observed. :param args: """ logging.debug('starting test check_for_statistic_publications') try: future = Future() node = StatisticsListener(future, list(expected_nodes), num_msgs, expected_topic) rclpy.spin_until_future_complete( node, future, timeout_sec=PUBLICATION_TEST_TIMEOUT_SECONDS) if node.received_all_expected_messages(): logging.info('check_for_statistic_publications success') else: raise SystemMetricsEnd2EndTestException( 'check_for_statistic_publications failed.' ' Absent publisher(s): ' + str(node.lifecycle_nodes_message_counter)) finally: node.destroy_node()
def test_meng_wp_video(): # Run bag node to test rosbag_location = "./test/testing_resources/rosbag/test.bag" p = Process( target=play_rosbag, args=( rosbag_location, False, "--topics /terrasentia/usb_cam_node/image_raw --wait-for-subscribers", ), ) future = Future() waypointPublisher = WaypointPublisherTester(5, future) goal = get_goal() waypointPublisher.goal = goal waypointPublisher.goal_show = goal[6] p.start() rclpy.spin_until_future_complete(waypointPublisher, future) waypointPublisher.destroy_node() kill_testbag_cmd = ( "export PYTHONPATH= && . /opt/ros/melodic/setup.sh && rosnode list " + "| grep play | xargs rosnode kill") subprocess.Popen( kill_testbag_cmd, stdout=subprocess.DEVNULL, stderr=subprocess.STDOUT, shell=True, ) os.kill(p.pid, signal.SIGKILL) assert len(glob.glob("./test/results/wp/*")) != 0 assert waypointPublisher.twistflag
def main(args): if not args.csv: truncate_length = args.truncate_length if not args.full_length else None callback = subscriber_cb(truncate_length, args.no_arr, args.no_str) else: truncate_length = args.truncate_length if not args.full_length else None callback = subscriber_cb_csv(truncate_length, args.no_arr, args.no_str) qos_profile = qos_profile_from_short_keys(args.qos_profile, reliability=args.qos_reliability, durability=args.qos_durability, depth=args.qos_depth, history=args.qos_history) with NodeStrategy(args) as node: if args.message_type is None: message_type = get_msg_class(node, args.topic_name, include_hidden_topics=True) else: message_type = get_message(args.message_type) if message_type is None: raise RuntimeError( 'Could not determine the type for the passed topic') future = None if args.once: future = Future() callback = subscriber_cb_once_decorator(callback, future) subscriber(node, args.topic_name, message_type, callback, qos_profile, args.lost_messages, future, args.timeout)
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 start_spin_thread(self, waitable): waitable.future = Future(executor=self.executor) self.thr = threading.Thread( target=self.executor.spin_until_future_complete, args=(waitable.future, ), daemon=True) self.thr.start() return self.thr
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 test_default_incompatible_qos_callbacks(self): original_logger = rclpy.logging._root_logger pub_log_msg = None sub_log_msg = None log_msgs_future = Future() class MockLogger: def get_child(self, name): return self def warn(self, message, once=False): nonlocal pub_log_msg, sub_log_msg, log_msgs_future if message.startswith('New subscription discovered'): pub_log_msg = message elif message.startswith('New publisher discovered'): sub_log_msg = message if pub_log_msg is not None and sub_log_msg is not None: log_msgs_future.set_result(True) rclpy.logging._root_logger = MockLogger() qos_profile_publisher = QoSProfile( depth=10, durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_VOLATILE) self.node.create_publisher(EmptyMsg, self.topic_name, qos_profile_publisher) message_callback = Mock() qos_profile_subscription = QoSProfile( depth=10, durability=QoSDurabilityPolicy. RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) self.node.create_subscription(EmptyMsg, self.topic_name, message_callback, qos_profile_subscription) executor = rclpy.executors.SingleThreadedExecutor(context=self.context) rclpy.spin_until_future_complete(self.node, log_msgs_future, executor, 10.0) if not self.is_fastrtps: self.assertEqual( pub_log_msg, "New subscription discovered on topic '{}', requesting incompatible QoS. " 'No messages will be sent to it. ' 'Last incompatible policy: DURABILITY_QOS_POLICY'.format( self.topic_name)) self.assertEqual( sub_log_msg, "New publisher discovered on topic '{}', offering incompatible QoS. " 'No messages will be received from it. ' 'Last incompatible policy: DURABILITY_QOS_POLICY'.format( self.topic_name)) rclpy.logging._root_logger = original_logger
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 call_async(self, req): """ Make a service request and asyncronously get the result. :return: a Future instance that completes when the request does :rtype: :class:`rclpy.task.Future` instance """ sequence_number = _rclpy.rclpy_send_request(self.client_handle, req) if sequence_number in self._pending_requests: raise RuntimeError('Sequence (%r) conflicts with pending request' % sequence_number) future = Future() self._pending_requests[sequence_number] = future future.add_done_callback(self.remove_pending_request) return future
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 call_async(self, request: SrvTypeRequest) -> Future: """ Make a service request and asyncronously get the result. :param request: The service request. :return: A future that completes when the request does. """ sequence_number = _rclpy.rclpy_send_request(self.client_handle, request) if sequence_number in self._pending_requests: raise RuntimeError('Sequence (%r) conflicts with pending request' % sequence_number) future = Future() self._pending_requests[sequence_number] = future future.add_done_callback(self.remove_pending_request) return future