def main(args=None): rclpy.init(args=args) try: talker = Talker() listener = Listener() # Runs all callbacks in the main thread executor = SingleThreadedExecutor() # Add imported nodes to this executor executor.add_node(talker) executor.add_node(listener) try: # Execute callbacks for both nodes as they become ready executor.spin() finally: executor.shutdown() listener.destroy_node() talker.destroy_node() finally: rclpy.shutdown()
def setUpClass(cls): cls.context = rclpy.context.Context() rclpy.init(context=cls.context) cls.executor = SingleThreadedExecutor(context=cls.context) cls.node = rclpy.create_node('TestActionClient', context=cls.context) cls.mock_action_server = MockActionServer(cls.node)
class TestROS2TopicEchoPub(unittest.TestCase): # TODO(hidmic): investigate why making use of the same rclpy node, executor # and context for all tests on a per rmw implementation basis # makes them fail on Linux-aarch64 when using 'rmw_opensplice_cpp'. # Presumably, interfaces creation/destruction and/or executor spinning # on one test is affecting the other. def setUp(self): self.context = rclpy.context.Context() rclpy.init(context=self.context) self.node = rclpy.create_node(TEST_NODE, namespace=TEST_NAMESPACE, context=self.context) self.executor = SingleThreadedExecutor(context=self.context) self.executor.add_node(self.node) def tearDown(self): self.node.destroy_node() rclpy.shutdown(context=self.context) @launch_testing.markers.retry_on_failure(times=5) def test_pub_basic(self, launch_service, proc_info, proc_output): params = [('/clitest/topic/pub_basic', False, True), ('/clitest/topic/pub_compatible_qos', True, True), ('/clitest/topic/pub_incompatible_qos', True, False)] for topic, provide_qos, compatible_qos in params: with self.subTest(topic=topic, provide_qos=provide_qos, compatible_qos=compatible_qos): # Check for inconsistent arguments assert provide_qos if not compatible_qos else True received_message_count = 0 expected_minimum_message_count = 1 expected_maximum_message_count = 5 pub_extra_options = [] subscription_qos_profile = 10 if provide_qos: if compatible_qos: # For compatible test, put publisher at very high quality # and subscription at low pub_extra_options = [ '--qos-reliability', 'reliable', '--qos-durability', 'transient_local' ] subscription_qos_profile = QoSProfile( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, durability=DurabilityPolicy.VOLATILE) else: # For an incompatible example, reverse the quality extremes # and expect no messages to arrive pub_extra_options = [ '--qos-reliability', 'best_effort', '--qos-durability', 'volatile' ] subscription_qos_profile = QoSProfile( depth=10, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL) expected_maximum_message_count = 0 expected_minimum_message_count = 0 future = rclpy.task.Future() def message_callback(msg): """If we receive one message, the test has succeeded.""" nonlocal received_message_count received_message_count += 1 future.set_result(True) subscription = self.node.create_subscription( String, topic, message_callback, subscription_qos_profile) assert subscription try: command_action = ExecuteProcess( cmd=(['ros2', 'topic', 'pub'] + pub_extra_options + [topic, 'std_msgs/String', 'data: hello']), additional_env={'PYTHONUNBUFFERED': '1'}, output='screen') with launch_testing.tools.launch_process( launch_service, command_action, proc_info, proc_output, output_filter=launch_testing_ros.tools. basic_output_filter( filtered_rmw_implementation= get_rmw_implementation_identifier( ))) as command: self.executor.spin_until_future_complete( future, timeout_sec=10) command.wait_for_shutdown(timeout=10) # Check results assert ( received_message_count >= expected_minimum_message_count and received_message_count <= expected_maximum_message_count), \ ('Received {} messages from pub on {},' 'which is not in expected range {}-{}').format( received_message_count, topic, expected_minimum_message_count, expected_maximum_message_count ) finally: # Cleanup self.node.destroy_subscription(subscription) @launch_testing.markers.retry_on_failure(times=5) def test_pub_times(self, launch_service, proc_info, proc_output): command_action = ExecuteProcess( cmd=([ 'ros2', 'topic', 'pub', '-t', '5', '-w', '0', '/clitest/topic/pub_times', 'std_msgs/String', 'data: hello' ]), additional_env={'PYTHONUNBUFFERED': '1'}, output='screen') with launch_testing.tools.launch_process( launch_service, command_action, proc_info, proc_output, output_filter=launch_testing_ros.tools.basic_output_filter( filtered_rmw_implementation= get_rmw_implementation_identifier())) as command: assert command.wait_for_shutdown(timeout=10) assert command.exit_code == launch_testing.asserts.EXIT_OK assert launch_testing.tools.expect_output(expected_lines=[ 'publisher: beginning loop', "publishing #1: std_msgs.msg.String(data='hello')", '', "publishing #2: std_msgs.msg.String(data='hello')", '', "publishing #3: std_msgs.msg.String(data='hello')", '', "publishing #4: std_msgs.msg.String(data='hello')", '', "publishing #5: std_msgs.msg.String(data='hello')", '', ], text=command.output, strict=True) @launch_testing.markers.retry_on_failure(times=5) def test_echo_basic(self, launch_service, proc_info, proc_output): params = [ ('/clitest/topic/echo_basic', False, True, False), ('/clitest/topic/echo_compatible_qos', True, True, False), ('/clitest/topic/echo_incompatible_qos', True, False, False), ('/clitest/topic/echo_message_lost', False, True, True), ] for topic, provide_qos, compatible_qos, message_lost in params: with self.subTest(topic=topic, provide_qos=provide_qos, compatible_qos=compatible_qos): # Check for inconsistent arguments assert provide_qos if not compatible_qos else True echo_extra_options = [] publisher_qos_profile = 10 if provide_qos: if compatible_qos: # For compatible test, put publisher at very high quality # and subscription at low echo_extra_options = [ '--qos-reliability', 'best_effort', '--qos-durability', 'volatile' ] publisher_qos_profile = QoSProfile( depth=10, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL) else: # For an incompatible example, reverse the quality extremes # and expect no messages to arrive echo_extra_options = [ '--qos-reliability', 'reliable', '--qos-durability', 'transient_local' ] publisher_qos_profile = QoSProfile( depth=10, reliability=ReliabilityPolicy.BEST_EFFORT, durability=DurabilityPolicy.VOLATILE) if not message_lost: echo_extra_options.append('--no-lost-messages') publisher = self.node.create_publisher(String, topic, publisher_qos_profile) assert publisher def publish_message(): publisher.publish(String(data='hello')) publish_timer = self.node.create_timer(0.5, publish_message) try: command_action = ExecuteProcess( cmd=(['ros2', 'topic', 'echo'] + echo_extra_options + [topic, 'std_msgs/String']), additional_env={'PYTHONUNBUFFERED': '1'}, output='screen') with launch_testing.tools.launch_process( launch_service, command_action, proc_info, proc_output, output_filter=launch_testing_ros.tools. basic_output_filter( filtered_rmw_implementation= get_rmw_implementation_identifier( ))) as command: # The future won't complete - we will hit the timeout self.executor.spin_until_future_complete( rclpy.task.Future(), timeout_sec=5) command.wait_for_shutdown(timeout=10) # Check results if compatible_qos: assert command.output, 'Echo CLI printed no output' assert 'data: hello' in command.output.splitlines(), ( 'Echo CLI did not print expected message') else: assert command.output, ( 'Echo CLI did not print incompatible QoS warning') assert ( "New publisher discovered on topic '{}', offering incompatible" ' QoS.'.format(topic) in command.output ), ('Echo CLI did not print expected incompatible QoS warning' ) finally: # Cleanup self.node.destroy_timer(publish_timer) self.node.destroy_publisher(publisher) @launch_testing.markers.retry_on_failure(times=1) def test_echo_filter(self, launch_service, proc_info, proc_output): params = [ ('/clitest/topic/echo_filter_all_pass', "m.data=='hello'", True), ('/clitest/topic/echo_filter_all_filtered', "m.data=='success'", False), ] for topic, filter_expr, has_output in params: with self.subTest(topic=topic, filter_expr=filter_expr, print_count=10): # Check for inconsistent arguments publisher = self.node.create_publisher(String, topic, 10) assert publisher def publish_message(): publisher.publish(String(data='hello')) publish_timer = self.node.create_timer(0.5, publish_message) try: command_action = ExecuteProcess( cmd=(['ros2', 'topic', 'echo'] + ['--filter', filter_expr] + [topic, 'std_msgs/String']), additional_env={'PYTHONUNBUFFERED': '1'}, output='screen') with launch_testing.tools.launch_process( launch_service, command_action, proc_info, proc_output, output_filter=launch_testing_ros.tools. basic_output_filter( filtered_rmw_implementation= get_rmw_implementation_identifier( ))) as command: # The future won't complete - we will hit the timeout self.executor.spin_until_future_complete( rclpy.task.Future(), timeout_sec=5) command.wait_for_shutdown(timeout=10) # Check results if has_output: assert 'hello' in command.output, 'Echo CLI did not output' else: assert 'hello' not in command.output, 'All messages should be filtered out' finally: # Cleanup self.node.destroy_timer(publish_timer) self.node.destroy_publisher(publisher) @launch_testing.markers.retry_on_failure(times=5) def test_echo_raw(self, launch_service, proc_info, proc_output): topic = '/clitest/topic/echo_raw' publisher = self.node.create_publisher(String, topic, 10) assert publisher def publish_message(): publisher.publish(String(data='hello')) publish_timer = self.node.create_timer(0.5, publish_message) try: command_action = ExecuteProcess( cmd=[ 'ros2', 'topic', 'echo', '--raw', topic, 'std_msgs/msg/String' ], additional_env={'PYTHONUNBUFFERED': '1'}, output='screen') with launch_testing.tools.launch_process( launch_service, command_action, proc_info, proc_output, output_filter=launch_testing_ros.tools.basic_output_filter( filtered_rmw_implementation= get_rmw_implementation_identifier())) as command: # The future won't complete - we will hit the timeout self.executor.spin_until_future_complete(rclpy.task.Future(), timeout_sec=5) assert command.wait_for_output( functools.partial( launch_testing.tools.expect_output, expected_lines=[ "b'\\x00\\x01\\x00\\x00\\x06\\x00\\x00\\x00hello\\x00\\x00\\x00'", '---', ], strict=True), timeout=10), 'Echo CLI did not print expected message' assert command.wait_for_shutdown(timeout=10) finally: # Cleanup self.node.destroy_timer(publish_timer) self.node.destroy_publisher(publisher) @launch_testing.markers.retry_on_failure(times=5) def test_echo_once(self, launch_service, proc_info, proc_output): topic = '/clitest/topic/echo_once' publisher = self.node.create_publisher(String, topic, 10) assert publisher def publish_message(): publisher.publish(String(data='hello')) publish_timer = self.node.create_timer(1.0, publish_message) try: command_action = ExecuteProcess( cmd=[ 'ros2', 'topic', 'echo', '--once', topic, 'std_msgs/msg/String' ], additional_env={'PYTHONUNBUFFERED': '1'}, output='screen') with launch_testing.tools.launch_process( launch_service, command_action, proc_info, proc_output, output_filter=launch_testing_ros.tools.basic_output_filter( filtered_rmw_implementation= get_rmw_implementation_identifier())) as command: # The future won't complete - we will hit the timeout self.executor.spin_until_future_complete(rclpy.task.Future(), timeout_sec=3) assert command.wait_for_shutdown(timeout=5) assert launch_testing.tools.expect_output(expected_lines=[ 'data: hello', '---', ], text=command.output, strict=True) finally: # Cleanup self.node.destroy_timer(publish_timer) self.node.destroy_publisher(publisher)
def __init__(self, node_name, **kwargs): Node.__init__(self, node_name, **kwargs) self.__executor = SingleThreadedExecutor()
def main(args=None): rclpy.init(args=args) try: executor = SingleThreadedExecutor() node_a = FakeLifecycleNode('A') node_b = FakeLifecycleNode('B') executor.add_node(node_a) executor.add_node(node_b) lc = LifecycleClient() try: lc.configure_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) sleep(2) # give the system some time to converge lc.activate_system() executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) executor.spin_once(timeout_sec=1) sleep(2) # give the system some time to converge lc.change_mode('CC') executor.spin() finally: executor.shutdown() node_a.destroy_node() node_b.destroy_node() finally: rclpy.shutdown() return 0
def main(args=None): # Argument parsing and usage parser = get_parser() parsed_args = parser.parse_args() # Configuration variables qos_policy_name = parsed_args.incompatible_qos_policy_name qos_profile_publisher = QoSProfile(depth=10) qos_profile_subscription = QoSProfile(depth=10) if qos_policy_name == 'durability': print('Durability incompatibility selected.\n' 'Incompatibility condition: publisher durability kind <' 'subscripition durability kind.\n' 'Setting publisher durability to: VOLATILE\n' 'Setting subscription durability to: TRANSIENT_LOCAL\n') qos_profile_publisher.durability = \ QoSDurabilityPolicy.VOLATILE qos_profile_subscription.durability = \ QoSDurabilityPolicy.TRANSIENT_LOCAL elif qos_policy_name == 'deadline': print( 'Deadline incompatibility selected.\n' 'Incompatibility condition: publisher deadline > subscription deadline.\n' 'Setting publisher durability to: 2 seconds\n' 'Setting subscription durability to: 1 second\n') qos_profile_publisher.deadline = Duration(seconds=2) qos_profile_subscription.deadline = Duration(seconds=1) elif qos_policy_name == 'liveliness_policy': print('Liveliness Policy incompatibility selected.\n' 'Incompatibility condition: publisher liveliness policy <' 'subscripition liveliness policy.\n' 'Setting publisher liveliness policy to: AUTOMATIC\n' 'Setting subscription liveliness policy to: MANUAL_BY_TOPIC\n') qos_profile_publisher.liveliness = \ QoSLivelinessPolicy.AUTOMATIC qos_profile_subscription.liveliness = \ QoSLivelinessPolicy.MANUAL_BY_TOPIC elif qos_policy_name == 'liveliness_lease_duration': print( 'Liveliness lease duration incompatibility selected.\n' 'Incompatibility condition: publisher liveliness lease duration >' 'subscription liveliness lease duration.\n' 'Setting publisher liveliness lease duration to: 2 seconds\n' 'Setting subscription liveliness lease duration to: 1 second\n') qos_profile_publisher.liveliness_lease_duration = Duration(seconds=2) qos_profile_subscription.liveliness_lease_duration = Duration( seconds=1) elif qos_policy_name == 'reliability': print( 'Reliability incompatibility selected.\n' 'Incompatibility condition: publisher reliability < subscripition reliability.\n' 'Setting publisher reliability to: BEST_EFFORT\n' 'Setting subscription reliability to: RELIABLE\n') qos_profile_publisher.reliability = \ QoSReliabilityPolicy.BEST_EFFORT qos_profile_subscription.reliability = \ QoSReliabilityPolicy.RELIABLE else: print('{name} not recognised.'.format(name=qos_policy_name)) parser.print_help() return 0 # Initialization and configuration rclpy.init(args=args) topic = 'incompatible_qos_chatter' num_msgs = 5 publisher_callbacks = PublisherEventCallbacks( incompatible_qos=lambda event: get_logger('Talker').info(str(event))) subscription_callbacks = SubscriptionEventCallbacks( incompatible_qos=lambda event: get_logger('Listener').info(str(event))) try: talker = Talker(topic, qos_profile_publisher, event_callbacks=publisher_callbacks, publish_count=num_msgs) listener = Listener(topic, qos_profile_subscription, event_callbacks=subscription_callbacks) except UnsupportedEventTypeError as exc: print() print(exc, end='\n\n') print('Please try this demo using a different RMW implementation') return 1 executor = SingleThreadedExecutor() executor.add_node(listener) executor.add_node(talker) try: while talker.publish_count < num_msgs: executor.spin_once() except KeyboardInterrupt: pass except ExternalShutdownException: return 1 finally: rclpy.try_shutdown() return 0
def test_lifecycle_services(request): lc_node_name = 'test_lifecycle_services_lifecycle' lc_node = LifecycleNode(lc_node_name) client_node = Node('test_lifecycle_services_client') get_state_cli = client_node.create_client(lifecycle_msgs.srv.GetState, f'/{lc_node_name}/get_state') change_state_cli = client_node.create_client( lifecycle_msgs.srv.ChangeState, f'/{lc_node_name}/change_state') get_available_states_cli = client_node.create_client( lifecycle_msgs.srv.GetAvailableStates, f'/{lc_node_name}/get_available_states') get_available_transitions_cli = client_node.create_client( lifecycle_msgs.srv.GetAvailableTransitions, f'/{lc_node_name}/get_available_transitions') get_transition_graph_cli = client_node.create_client( lifecycle_msgs.srv.GetAvailableTransitions, f'/{lc_node_name}/get_transition_graph') for cli in ( get_state_cli, change_state_cli, get_available_states_cli, get_available_transitions_cli, get_transition_graph_cli, ): assert cli.wait_for_service(5.) # lunch a thread to spin the executor, so we can make sync service calls easily executor = SingleThreadedExecutor() executor.add_node(client_node) executor.add_node(lc_node) thread = Thread(target=executor.spin) thread.start() def cleanup(): # Stop executor and join thread. # This cleanup is run even if an assertion fails. executor.shutdown() thread.join() request.addfinalizer(cleanup) # test all services req = lifecycle_msgs.srv.GetState.Request() resp = get_state_cli.call(req) assert resp.current_state.label == 'unconfigured' req = lifecycle_msgs.srv.ChangeState.Request() req.transition.id = lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE resp = change_state_cli.call(req) assert resp.success req = lifecycle_msgs.srv.GetState.Request() resp = get_state_cli.call(req) assert resp.current_state.label == 'inactive' req = lifecycle_msgs.srv.GetAvailableStates.Request() resp = get_available_states_cli.call(req) states_labels = {state.label for state in resp.available_states} assert states_labels == { 'unknown', 'unconfigured', 'inactive', 'active', 'finalized', 'configuring', 'cleaningup', 'shuttingdown', 'activating', 'deactivating', 'errorprocessing', '' } req = lifecycle_msgs.srv.GetAvailableTransitions.Request() resp = get_available_transitions_cli.call(req) transitions_labels = { transition_def.transition.label for transition_def in resp.available_transitions } assert transitions_labels == {'activate', 'cleanup', 'shutdown'} req = lifecycle_msgs.srv.GetAvailableTransitions.Request() resp = get_transition_graph_cli.call(req) transitions_labels = { transition_def.transition.label for transition_def in resp.available_transitions } assert transitions_labels == { 'configure', 'activate', 'cleanup', 'shutdown', 'deactivate', 'transition_error', 'transition_failure', 'transition_success' }
def _run(self): from rclpy.executors import SingleThreadedExecutor executor = SingleThreadedExecutor(context=self._context) executor.add_node(self._node) while self._run: executor.spin_once(timeout_sec=1.0)
def test_add_node_to_executor(self): self.assertIsNotNone(self.node.handle) executor = SingleThreadedExecutor(context=self.context) executor.add_node(self.node) self.assertIn(self.node, executor.get_nodes())
def test_executor_add_node(self): self.assertIsNotNone(self.node.handle) executor = SingleThreadedExecutor(context=self.context) assert executor.add_node(self.node) assert not executor.add_node(self.node)
def test_create_task_dependent_coroutines(self): self.assertIsNotNone(self.node.handle) executor = SingleThreadedExecutor(context=self.context) executor.add_node(self.node) async def coro1(): return 'Sentinel Result 1' future1 = executor.create_task(coro1) async def coro2(): nonlocal future1 await future1 return 'Sentinel Result 2' future2 = executor.create_task(coro2) # Coro2 is newest task, so it gets to await future1 in this spin executor.spin_once(timeout_sec=0) # Coro1 execs in this spin executor.spin_once(timeout_sec=0) self.assertTrue(future1.done()) self.assertEqual('Sentinel Result 1', future1.result()) self.assertFalse(future2.done()) # Coro2 passes the await step here (timeout change forces new generator) executor.spin_once(timeout_sec=1) self.assertTrue(future2.done()) self.assertEqual('Sentinel Result 2', future2.result())
class TurtlebotEnv(): def __init__(self): self.node_ = rclpy.create_node('turtlebot3_env') self.executor = SingleThreadedExecutor() self.executor.add_node(self.node_) self.act = 0 self.done = False self.actions = [ [parameters.LINEAR_FWD_VELOCITY, parameters.ANGULAR_FWD_VELOCITY], [parameters.LINEAR_STR_VELOCITY, parameters.ANGULAR_VELOCITY], [parameters.LINEAR_STR_VELOCITY, -parameters.ANGULAR_VELOCITY] ] self.bonous_reward = 0 self.collision = False self.collision_tol = 0.125 self.laser_scan_range = [0] * 360 self.states_input = [3.5] * 8 self.zero_div_tol = 0.01 self.range_min = 0.0 self.pub_cmd_vel = self.node_.create_publisher(Twist, 'cmd_vel', 1) self.sub_scan = self.node_.create_subscription( LaserScan, 'scan', self.scan_callback, qos_profile_sensor_data) self.reset_simulation = self.node_.create_client( Empty, 'reset_simulation') self.reset_world = self.node_.create_client(Empty, 'reset_world') self.unpause_proxy = self.node_.create_client(Empty, 'unpause_physics') self.pause_proxy = self.node_.create_client(Empty, 'pause_physics') self.get_entity_state = self.node_.create_client( GetEntityState, 'get_entity_state') self.set_entity_state = self.node_.create_client( SetEntityState, 'set_entity_state') self.scan_msg_received = False self.t = Thread(target=self.executor.spin) self.t.start() def cleanup(self): self.t.join() def get_reward(self): reward = 0 if self.collision is True: reward = -10 self.done = True return reward, self.done elif self.collision is False and self.act is 0: if abs(min(self.states_input)) >= self.zero_div_tol: reward = 0.08 - (1 / (min(self.states_input)**2)) * 0.005 else: reward = -10 if reward > 0: self.bonous_reward += reward reward = self.bonous_reward else: bonous_discount_factor = 0.6 self.bonous_reward *= bonous_discount_factor if abs(min(self.states_input)) >= self.zero_div_tol: reward = 0.02 - (1 / min(self.states_input)) * 0.005 else: reward = -10 return reward, self.done def scan_callback(self, LaserScan): self.scan_msg_received = True self.laser_scan_range = [] self.range_min = LaserScan.range_min range_max = LaserScan.range_max for i in range(len(LaserScan.ranges)): if LaserScan.ranges[i] == float('Inf'): self.laser_scan_range.append(range_max) elif LaserScan.ranges[i] < self.range_min: self.laser_scan_range.append(self.range_min + self.collision_tol) else: self.laser_scan_range.append(LaserScan.ranges[i]) if self.check_collision(): self.collision = True self.done = True self.states_input = [] for i in range(8): step = int(len(LaserScan.ranges) / 8) self.states_input.append( min(self.laser_scan_range[i * step:(i + 1) * step], default=0)) def action_space(self): return len(self.actions) def step(self, action): vel_cmd = Twist() self.act = action vel_cmd.linear.x = self.actions[action][0] vel_cmd.angular.z = self.actions[action][1] self.pub_cmd_vel.publish(vel_cmd) vel_cmd.linear.x = 0.0 vel_cmd.angular.z = 0.0 get_reward = self.get_reward() return self.states_input, get_reward[0], self.done def check_collision(self): if min(self.laser_scan_range) < self.range_min + self.collision_tol: print("Near collision detected... " + str(min(self.laser_scan_range))) return True return False def stop_action(self): vel_cmd = Twist() vel_cmd.linear.x = 0.0 vel_cmd.angular.z = 0.0 self.pub_cmd_vel.publish(vel_cmd) def reset(self): self.scan_msg_received = False self.stop_action while not self.reset_world.wait_for_service(timeout_sec=1.0): print('Reset world service is not available...') self.reset_world.call_async(Empty.Request()) while not self.reset_simulation.wait_for_service(timeout_sec=1.0): print('Reset simulation service is not available...') self.reset_simulation.call_async(Empty.Request()) self.laser_scan_range = [0] * 360 self.states_input = [3.5] * 8 while not self.scan_msg_received and rclpy.ok(): sleep(0.1) self.collision = False self.done = False self.bonous_reward = 0 return self.states_input
class TestCreateWhileSpinning(unittest.TestCase): """ Test that the executor wakes after an entity is created. This is a regression test for ros2/rclpy#188. """ def setUp(self): rclpy.init() self.node = rclpy.create_node('TestCreateWhileSpinning', namespace='/rclpy') self.executor = SingleThreadedExecutor() self.executor.add_node(self.node) self.exec_thread = threading.Thread(target=self.executor.spin) self.exec_thread.start() # Make sure executor is blocked by rcl_wait time.sleep(TIMEOUT) def tearDown(self): self.executor.shutdown() rclpy.shutdown() self.exec_thread.join() self.node.destroy_node() def test_publish_subscribe(self): evt = threading.Event() self.node.create_subscription(BasicTypes, 'foo', lambda msg: evt.set(), 1) pub = self.node.create_publisher(BasicTypes, 'foo', 1) pub.publish(BasicTypes()) assert evt.wait(TIMEOUT) def test_client_server(self): evt = threading.Event() def trigger_event(req, resp): nonlocal evt evt.set() return resp self.node.create_service(BasicTypesSrv, 'foo', trigger_event) cli = self.node.create_client(BasicTypesSrv, 'foo') cli.wait_for_service() cli.call_async(BasicTypesSrv.Request()) assert evt.wait(TIMEOUT) def test_guard_condition(self): evt = threading.Event() guard = self.node.create_guard_condition(lambda: evt.set()) guard.trigger() assert evt.wait(TIMEOUT) def test_timer(self): evt = threading.Event() self.node.create_timer(TIMEOUT / 10, lambda: evt.set()) assert evt.wait(TIMEOUT) def test_waitable(self): evt = threading.Event() class DummyWaitable(Waitable): def __init__(self): super().__init__(ReentrantCallbackGroup()) def is_ready(self, wait_set): return False def take_data(self): return None async def execute(self, taken_data): pass def get_num_entities(self): return NumberOfEntities(0, 0, 0, 0, 0) def add_to_wait_set(self, wait_set): nonlocal evt evt.set() pass self.node.add_waitable(DummyWaitable()) assert evt.wait(TIMEOUT)
class WaitForTopics: """ Wait to receive messages on supplied topics. Example usage: -------------- from std_msgs.msg import String # Method 1, using the 'with' keyword def method_1(): topic_list = [('topic_1', String), ('topic_2', String)] with WaitForTopics(topic_list, timeout=5.0): # 'topic_1' and 'topic_2' received at least one message each print('Given topics are receiving messages !') # Method 2, calling wait() and shutdown() manually def method_2(): topic_list = [('topic_1', String), ('topic_2', String)] wait_for_topics = WaitForTopics(topic_list, timeout=5.0) assert wait_for_topics.wait() print('Given topics are receiving messages !') print(wait_for_topics.topics_not_received()) # Should be an empty set print(wait_for_topics.topics_received()) # Should be {'topic_1', 'topic_2'} wait_for_topics.shutdown() """ def __init__(self, topic_tuples, timeout=5.0): self.topic_tuples = topic_tuples self.timeout = timeout self.__ros_context = rclpy.Context() rclpy.init(context=self.__ros_context) self.__ros_executor = SingleThreadedExecutor( context=self.__ros_context) self._prepare_ros_node() # Start spinning self.__running = True self.__ros_spin_thread = Thread(target=self._spin_function) self.__ros_spin_thread.start() def _prepare_ros_node(self): node_name = '_test_node_' +\ ''.join(random.choices(string.ascii_uppercase + string.digits, k=10)) self.__ros_node = _WaitForTopicsNode(name=node_name, node_context=self.__ros_context) self.__ros_executor.add_node(self.__ros_node) def _spin_function(self): while self.__running: self.__ros_executor.spin_once(1.0) def wait(self): self.__ros_node.start_subscribers(self.topic_tuples) return self.__ros_node.msg_event_object.wait(self.timeout) def shutdown(self): self.__running = False self.__ros_spin_thread.join() self.__ros_node.destroy_node() rclpy.shutdown(context=self.__ros_context) def topics_received(self): """Topics that received at least one message.""" return self.__ros_node.received_topics def topics_not_received(self): """Topics that did not receive any messages.""" return self.__ros_node.expected_topics - self.__ros_node.received_topics def __enter__(self): if not self.wait(): raise RuntimeError('Did not receive messages on these topics: ', self.topics_not_received()) return self def __exit__(self, exep_type, exep_value, trace): if exep_type is not None: raise Exception('Exception occured, value: ', exep_value) self.shutdown()
class ROSAdapter: """Wraps rclpy API to ease ROS node usage in `launch_ros` actions.""" def __init__(self, *, argv: Optional[List[str]] = None, autostart: bool = True): """ Construct adapter. :param: argv List of global arguments for rclpy context initialization. :param: autostart Whether to start adapter on construction or not. """ # Do not use `None` here, as `rclpy.init` will use `sys.argv` in that case. self.__argv = [] if argv is None else argv self.__ros_context = None self.__ros_node = None self.__ros_executor = None self.__is_running = False if autostart: self.start() def start(self): """Start ROS adapter.""" if self.__is_running: raise RuntimeError( 'Cannot start a ROS adapter that is already running') self.__ros_context = rclpy.Context() rclpy.init(args=self.__argv, context=self.__ros_context) self.__ros_node = rclpy.create_node('launch_ros_{}'.format( os.getpid()), context=self.__ros_context) self.__ros_executor = SingleThreadedExecutor( context=self.__ros_context) self.__is_running = True self.__ros_executor_thread = threading.Thread(target=self._run) self.__ros_executor_thread.start() def _run(self): try: self.__ros_executor.add_node(self.__ros_node) while self.__is_running: # TODO(wjwwood): switch this to `spin()` when it considers # asynchronously added subscriptions. # see: https://github.com/ros2/rclpy/issues/188 self.__ros_executor.spin_once(timeout_sec=1.0) except KeyboardInterrupt: pass finally: self.__ros_executor.remove_node(self.__ros_node) def shutdown(self): """Shutdown ROS adapter.""" if not self.__is_running: raise RuntimeError( 'Cannot shutdown a ROS adapter that is not running') self.__is_running = False self.__ros_executor_thread.join() self.__ros_node.destroy_node() rclpy.shutdown(context=self.__ros_context) @property def argv(self): return self.__argv @property def ros_context(self): return self.__ros_context @property def ros_node(self): return self.__ros_node @property def ros_executor(self): return self.__ros_executor
def setup_method(self): self.context = rclpy.context.Context() rclpy.init(context=self.context) self.node = rclpy.create_node('test_rate', context=self.context) self.executor = SingleThreadedExecutor(context=self.context) self.executor.add_node(self.node)
def setUpClass(cls): cls.context = rclpy.context.Context() rclpy.init(context=cls.context) cls.executor = SingleThreadedExecutor(context=cls.context) cls.node = rclpy.create_node('TestBufferClient', context=cls.context) cls.executor.add_node(cls.node)
class MoveAbsJointHandler(): def __init__(self, path, interval): super().__init__() self.pipe_path = path self.interval = interval self.result = ' ' self.count = 0 self.move = 0 self.executor = SingleThreadedExecutor() self.node = MoveAbsJointClient() self.executor.add_node(self.node) try: os.mkfifo(self.pipe_path) except OSError: print('[Info] MoveAbsJoint: Pipe: ', self.pipe_path, ' exists.') def rosHandler(self): while True: try: self.executor.spin_once() except ExternalShutdownException: try: rclpy.init() except: a = 1 def run(self): fd = os.open(self.pipe_path, os.O_CREAT | os.O_RDWR) thread_node = threading.Thread(target=self.rosHandler) thread_node.start() while True: try: data = os.read(fd, 400) if data.decode('utf-8').split(';')[0] == 'MoveAbsJoint': print('[Info] MoveAbsJoint gets request from Pipe.') try: msg = data.decode('utf-8').split(';')[1:-1] goal = MotionMoveAbsJoint.Goal() goal.id = 1 goal.joint = [0.0] * 8 joint = [float(x) for x in msg[0].split(',')[:]] for i in range(len(joint)): goal.joint[i] = joint[i] extjoint = [float(x) for x in msg[1].split(',')[:]] goal.extjoint = [0.0] * 8 for i in range(len(extjoint)): goal.extjoint[i] = extjoint[i] load = [float(x) for x in msg[2].split(',')[:]] goal.load = [0.0] * 10 for i in range(len(load)): goal.load[i] = load[i] goal.speed = float(msg[3]) goal.zone = float(msg[4]) time1 = time.time() self.node.send_goal(goal) self.count += 1 time2 = time.time() print('[Info] MoveAbsJoint sends request to ROS.') print('[Info] Goal:', goal.joint) print('[Info] Node send time', time2 - time1) while self.count != self.node._count: #print(self.count,self.node._count) time.sleep(self.interval / 2) self.result = self.node._ret time1 = time.time() print('[Info] MoveAbsJoint gets result from ROS.') print('[Info] Result:', self.result) print('[Info] Node spin time', time1 - time2) if self.result > 0: self.pipe_result = 'y' + ' ' * 399 elif self.result == 0: time1 = time.time() print( '[Info] MoveAbsJoint resends request from ROS.' ) retry = 0 while self.node._ret <= 0: time.sleep(self.interval * 16) self.node.send_goal(goal) self.count += 1 retry += 1 while self.count != self.node._count: time.sleep(self.interval / 2) time2 = time.time() print('[Info] Node resend times:', retry, '. Time spent:', time2 - time1) self.result = self.node._ret self.pipe_result = 'y' + ' ' * 399 else: self.pipe_result = 'n1' + ' ' * 398 print('[Error] MoveAbsJoint gets error result.') os.write(fd, self.pipe_result.encode('utf-8')) print('[Info] MoveAbsJoint sends result to Pipe.', self.result) self.move += 1 print('[Info] MoveAbsJoint counter', self.move) except: time.sleep(self.interval / 2) elif data.decode('utf-8')[0] == 'y' or data.decode( 'utf-8')[0] == 'n': print('[Info] MoveAbsJoint resends result to Pipe.') os.write(fd, data) time.sleep(self.interval) except: print('[Error] MoveAbsJoint.') time.sleep(self.interval / 2)
def test_action_client(self): """Test simple action states""" node = rclpy.create_node('simple_action_test') node.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) executor = SingleThreadedExecutor() def spin(): rclpy.spin(node, executor=executor) sq = Sequence(['succeeded', 'aborted', 'preempted', 'foobar'], 'succeeded') sq.userdata['g1'] = g1 sq.userdata['g2'] = g2 sq.userdata['order'] = 1 sq.userdata['goal_alias'] = 1 with sq: # Test single goal policy Sequence.add( 'GOAL_STATIC', SimpleActionState(node, "fibonacci", Fibonacci, goal=g1)) Sequence.add( 'GOAL_KEY', SimpleActionState(node, "fibonacci", Fibonacci, goal_key='g1')) Sequence.add( 'GOAL_SLOTS', SimpleActionState(node, "fibonacci", Fibonacci, goal_slots=['order'])) Sequence.add('GOAL_SLOTS_REMAP', SimpleActionState(node, "fibonacci", Fibonacci, goal_slots=['order']), remapping={'order': 'goal_alias'}) # Test goal callback def goal_cb_0(ud, default_goal): return Fibonacci.Goal(order=1) Sequence.add( 'GOAL_CB', SimpleActionState(node, "fibonacci", Fibonacci, goal_cb=goal_cb_0)) Sequence.add( 'GOAL_CB_LAMBDA', SimpleActionState( node, "fibonacci", Fibonacci, goal_cb=lambda ud, goal: Fibonacci.Goal(order=1))) Sequence.add( 'GOAL_CB_UD', SimpleActionState(node, "fibonacci", Fibonacci, goal_cb=lambda ud, goal: ud.g1, input_keys=['g1'])) @cb_interface(input_keys=['g1']) def goal_cb_1(ud, default_goal): return ud.g1 Sequence.add( 'GOAL_CB_UD_DECORATOR', SimpleActionState(node, "fibonacci", Fibonacci, goal_cb=goal_cb_1)) Sequence.add( 'GOAL_CB_ARGS', SimpleActionState( node, "fibonacci", Fibonacci, goal_cb=lambda ud, goal, g: Fibonacci.Goal(order=g), goal_cb_args=[1])) Sequence.add( 'GOAL_CB_KWARGS', SimpleActionState( node, "fibonacci", Fibonacci, goal_cb=lambda ud, goal, gg: Fibonacci.Goal(order=gg), goal_cb_kwargs={'gg': 1})) Sequence.add( 'GOAL_CB_ARGS_KWARGS', SimpleActionState(node, "fibonacci", Fibonacci, goal_cb=lambda ud, goal, g, gg: Fibonacci. Goal(order=(g - gg)), goal_cb_args=[2], goal_cb_kwargs={'gg': 1})) # Test overriding goal policies Sequence.add( 'GOAL_STATIC_SLOTS', SimpleActionState(node, "fibonacci", Fibonacci, goal=g2, goal_slots=['order'])) Sequence.add( 'GOAL_STATIC_CB', SimpleActionState( node, "fibonacci", Fibonacci, goal=g2, goal_cb=CBInterface( lambda ud, goal: setattr(goal, 'order', 1), output_keys=['goal']))) # Test result policies Sequence.add( 'RESULT_KEY', SimpleActionState(node, "fibonacci", Fibonacci, goal=g1, result_key='res_key')) Sequence.add('RESULT_KEY_CHECK', AssertUDState(['res_key'])) Sequence.add( 'RESULT_CB', SimpleActionState( node, "fibonacci", Fibonacci, goal=g1, result_cb=CBInterface( lambda ud, res_stat, res: setattr(ud, 'res_cb', res), output_keys=['res_cb']))) Sequence.add('RESULT_CB_CHECK', AssertUDState(['res_cb'])) Sequence.add( 'RESULT_SLOTS', SimpleActionState(node, "fibonacci", Fibonacci, goal=g1, result_slots=['sequence'])) Sequence.add('RESULT_SLOTS_CHECK', AssertUDState(['sequence'])) Sequence.add('RESULT_SLOTS_REMAP', SimpleActionState(node, "fibonacci", Fibonacci, goal=g1, result_slots=['sequence']), remapping={'sequence': 'res_alias'}) Sequence.add('RESULT_SLOTS_MAP_CHECK', AssertUDState(['res_alias'])) Sequence.add( 'RESULT_CB_OUTCOME', SimpleActionState(node, "fibonacci", Fibonacci, goal=g1, result_cb=CBInterface( lambda ud, res_stat, res: 'foobar', outcomes=['foobar']))) spinner = threading.Thread(target=spin) spinner.start() sq_outcome = sq.execute() assert sq_outcome == 'foobar'