def test_import_each_available_rmw_implementation(): from rclpy.impl.rmw_implementation_tools import get_rmw_implementations for rmw_implementation in get_rmw_implementations(): pool = multiprocessing.Pool(1) result = pool.apply( func=run_catch_report_raise, args=(func_import_each_available_rmw_implementation, rmw_implementation,) ) assert result pool.close() pool.join()
def func_import_each_available_rmw_implementation(rmw_implementation): import rclpy from rclpy.impl.rmw_implementation_tools import get_rmw_implementations from rclpy.impl.rmw_implementation_tools import select_rmw_implementation rmw_implementations = get_rmw_implementations() assert(rmw_implementation in rmw_implementations) select_rmw_implementation(rmw_implementation) rclpy.init([]) set_rmw = rclpy.get_rmw_implementation_identifier() assert set_rmw == rmw_implementation, \ "expected '{0}' but got '{1}'".format( rmw_implementation, set_rmw) return ctypes.c_bool(True)
def test_select_rmw_implementation_by_environment(): from rclpy.impl.rmw_implementation_tools import get_rmw_implementations orig_rclpy_implementation_env = os.environ.get('RCLPY_IMPLEMENTATION', None) for rmw_implementation in get_rmw_implementations(): pool = multiprocessing.Pool(1) result = pool.apply( func=run_catch_report_raise, args=(func_select_rmw_implementation_by_environment, rmw_implementation,) ) if orig_rclpy_implementation_env is None: if 'RCLPY_IMPLEMENTATION' in os.environ: del os.environ['RCLPY_IMPLEMENTATION'] else: os.environ['RCLPY_IMPLEMENTATION'] = orig_rclpy_implementation_env assert result, "type('{0}'): '{1}'".format(type(result), result) pool.close() pool.join()
def func_select_rmw_implementation_by_environment(rmw_implementation): import rclpy from rclpy.impl.rmw_implementation_tools import get_rmw_implementations os.environ['RCLPY_IMPLEMENTATION'] = rmw_implementation assert('RCLPY_IMPLEMENTATION' in os.environ) rmw_implementations = get_rmw_implementations() assert(os.environ['RCLPY_IMPLEMENTATION'] in rmw_implementations) rclpy.init([]) set_rmw = rclpy.get_rmw_implementation_identifier() assert set_rmw == rmw_implementation, \ "expected '{0}' but got '{1}'".format( rmw_implementation, set_rmw) return ctypes.c_bool(True)
def init(args=None): rclpy_rmw_env = os.getenv(RCLPY_IMPLEMENTATION_ENV_NAME, None) if rclpy_rmw_env is not None: available_rmw_implementations = rmw_implementation_tools.get_rmw_implementations( ) if rclpy_rmw_env not in available_rmw_implementations: logger = logging.getLogger('rclpy') logger.error( "The rmw implementation specified in 'RCLPY_IMPLEMENTATION={0}', " "is not one of the available implementations: {1}".format( rclpy_rmw_env, available_rmw_implementations)) raise InvalidRCLPYImplementation() rmw_implementation_tools.select_rmw_implementation(rclpy_rmw_env) # This line changes what is in "_rclpy" to be the rmw implementation module that was imported. implementation_singleton.set_rclpy_implementation( rmw_implementation_tools.import_rmw_implementation()) # Now we can use _rclpy to call the implementation specific rclpy_init(). return _rclpy.rclpy_init(args if args is not None else sys.argv)
def init(args=None): rclpy_rmw_env = os.getenv('RCLPY_IMPLEMENTATION', None) if rclpy_rmw_env is not None: available_rmw_implementations = rmw_implementation_tools.get_rmw_implementations() if rclpy_rmw_env not in available_rmw_implementations: logger = logging.getLogger('rclpy') logger.error( "The rmw implementation specified in 'RCLPY_IMPLEMENTATION={0}', " "is not one of the available implementations: {1}" .format(rclpy_rmw_env, available_rmw_implementations) ) raise InvalidRCLPYImplementation() rmw_implementation_tools.select_rmw_implementation(rclpy_rmw_env) # This line changes what is in "_rclpy" to be the rmw implementation module that was imported. implementation_singleton.set_rclpy_implementation( rmw_implementation_tools.import_rmw_implementation() ) # Now we can use _rclpy to call the implementation specific rclpy_init(). return _rclpy.rclpy_init(args if args is not None else sys.argv)
def func_import_each_available_rmw_implementation(rmw_implementation): import rclpy from rclpy.impl.rmw_implementation_tools import get_rmw_implementations from rclpy.impl.rmw_implementation_tools import select_rmw_implementation rmw_implementations = get_rmw_implementations() assert(rmw_implementation in rmw_implementations) select_rmw_implementation(rmw_implementation) rclpy.init([]) set_rmw = rclpy.get_rmw_implementation_identifier() assert set_rmw in identifiers_map, \ "expected identifier '{0}' to be in the map '{1}'".format(set_rmw, identifiers_map) set_rmw_equivalent = identifiers_map[set_rmw] assert set_rmw_equivalent == rmw_implementation, \ "expected '{0}' but got '{1}' (aka '{2}')".format( rmw_implementation, set_rmw_equivalent, set_rmw) return ctypes.c_bool(True)
def func_select_rmw_implementation_by_environment(rmw_implementation): import rclpy from rclpy.impl.rmw_implementation_tools import get_rmw_implementations os.environ['RCLPY_IMPLEMENTATION'] = rmw_implementation assert('RCLPY_IMPLEMENTATION' in os.environ) rmw_implementations = get_rmw_implementations() assert(os.environ['RCLPY_IMPLEMENTATION'] in rmw_implementations) rclpy.init([]) set_rmw = rclpy.get_rmw_implementation_identifier() assert set_rmw in identifiers_map, \ "expected identifier '{0}' to be in the map '{1}'".format(set_rmw, identifiers_map) set_rmw_equivalent = identifiers_map[set_rmw] assert set_rmw_equivalent == rmw_implementation, \ "expected '{0}' but got '{1}' (aka '{2}')".format( rmw_implementation, set_rmw_equivalent, set_rmw) return ctypes.c_bool(True)
qos_profile_default) spin_count = 1 print('subscriber: beginning loop') while (rclpy.ok() and spin_count < number_of_cycles and len(received_messages) != len(expected_msgs)): rclpy.spin_once(node) spin_count += 1 rclpy.shutdown() assert len(received_messages) == len(expected_msgs),\ 'Should have received {} {} messages from talker'.format(len(expected_msgs), message_name) if __name__ == '__main__': from rclpy.impl.rmw_implementation_tools import get_rmw_implementations rmw_implementations = get_rmw_implementations() parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('message_name', default='Primitives', help='name of the ROS message') parser.add_argument('-r', '--rmw_implementation', default=rmw_implementations[0], choices=rmw_implementations, help='rmw_implementation identifier') parser.add_argument('-n', '--number_of_cycles', type=int, default=10, help='number of sending attempts') args = parser.parse_args() try: listener( message_name=args.message_name, rmw_implementation=args.rmw_implementation, number_of_cycles=args.number_of_cycles) except KeyboardInterrupt: